// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_LENGTH_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_LENGTH_SPHERICAL_HPP

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/length/services.hpp>

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace length
{

template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
    : public strategies::detail::spherical_base<RadiusTypeOrSphere>
{
    using base_t = strategies::detail::spherical_base<RadiusTypeOrSphere>;

public:
    spherical() = default;

    template <typename RadiusOrSphere>
    explicit spherical(RadiusOrSphere const& radius_or_sphere)
        : base_t(radius_or_sphere)
    {}

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  distance::detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::haversine
                <
                    typename base_t::radius_type, CalculationType
                >(base_t::radius());
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, spherical_equatorial_tag>
{
    using type = strategies::length::spherical<>;
};

template <typename R, typename CT>
struct strategy_converter<strategy::distance::haversine<R, CT> >
{
    static auto get(strategy::distance::haversine<R, CT> const& s)
    {
        return strategies::length::spherical<R, CT>(s.radius());
    }
};

} // namespace services

}} // namespace strategies::length

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_LENGTH_SPHERICAL_HPP


/**
 * @file nonlinschroedingerequation.cc
 * @brief NPDE homework NonLinSchroedingerEquation code
 * @author Oliver Rietmann
 * @date 22.04.2020
 * @copyright Developed at ETH Zurich
 */

#include "nonlinschroedingerequation.h"

#include <cmath>

#include <Eigen/Core>

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>

namespace NonLinSchroedingerEquation {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix3d MassElementMatrixProvider::Eval(const lf::mesh::Entity &cell) {
  LF_VERIFY_MSG(cell.RefEl() == lf::base::RefEl::kTria(),
                "Unsupported cell type " << cell.RefEl());
  Eigen::Matrix3d element_matrix;
  //====================
  // Your code goes here
  //====================
  return element_matrix;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double Norm(const Eigen::VectorXcd &mu, const Eigen::SparseMatrix<double> &D) {
  //====================
  // Your code goes here
  // Replace this dummy value by the approximate
  // norm of the mesh function assiciated with mu
  return 1.0;
  //====================
}

double KineticEnergy(const Eigen::VectorXcd &mu,
                     const Eigen::SparseMatrix<double> &A) {
  //====================
  // Your code goes here
  // Replace this dummy value by the kinetic energy
  // of the mesh function assiciated with mu
  return 0.0;
  //====================
}

double InteractionEnergy(const Eigen::VectorXcd &mu,
                         const Eigen::SparseMatrix<double> &D) {
  //====================
  // Your code goes here
  // Replace this dummy value by the interaction
  // energy of the mesh function assiciated with mu
  return 0.0;
  //====================
}
/* SAM_LISTING_END_2 */

}  // namespace NonLinSchroedingerEquation


#include <urdf_model/model.h>

#include <kdl/chainiksolverpos_lma.hpp>

#include "ros/ros.h"

#include "applicable_pose_generator/pose_reachability_filter.h"
#include <boost/shared_ptr.hpp>

#include <kdl_parser/kdl_parser.hpp>
#include <eigen_conversions/eigen_kdl.h>

namespace CreateChain
{
chain_creation::chain_creation()
{
  ros::NodeHandle pnh("~");
  if (!pnh.getParam("robot_urdf", robot_urdf_)){
    ROS_ERROR("did not set parameter robot_urdf");
    exit (EXIT_FAILURE);
  }
  if (!pnh.getParam("base_link", base_link_param_)){
    ROS_ERROR("did not set parameter base_link");
    exit (EXIT_FAILURE);
  }
  if (!pnh.getParam("tool0", tool0_param_)){
    ROS_ERROR("did not set parameter tool0");
    exit (EXIT_FAILURE);
  }
  if (!Mymodel.initFile(robot_urdf_))
  {
    ROS_ERROR("Failed to parse urdf file");
    return;
  }
  else
  {
  ROS_INFO("Successfully parsed urdf file");
  }
  urdf::ModelInterfaceConstSharedPtr model_ptr = boost::make_shared<urdf::Model>(Mymodel);
  if(!CK.init(model_ptr,base_link_param_,tool0_param_,"Robot_from_urdf"))
  {
    ROS_ERROR("failed to initiat the chain");
    return;
  }
  kdl_parser::treeFromUrdfModel(*model_ptr, robot_tree);
  robot_tree.getChain(base_link_param_,tool0_param_,robot_chain);
}

bool chain_creation::chain_Parse(Eigen::Affine3d ei_transform_to_check)
{
  //get transform that we are trying to reach with robot
  KDL::ChainIkSolverPos_LMA solving_Ik(robot_chain);
  robot_joints.resize(robot_chain.getNrOfJoints());
  return_joint_values.resize(robot_chain.getNrOfJoints());
  tf::transformEigenToKDL(ei_transform_to_check,transform_goal_kdl);

  ROS_INFO("%d",solving_Ik.CartToJnt(robot_joints,transform_goal_kdl,return_joint_values));

  if(!solving_Ik.CartToJnt(robot_joints,transform_goal_kdl,return_joint_values))
  {
    ROS_INFO("Reachable Pose Found");
    return true;
  }
  else
  {
    ROS_INFO("Unreachable pose found");
    return false;
  }
}
}//end of namespace create_chain_take_pose_inverse_kinamatics


///////////////////////////////////////////////////////////////////////////////
// independence.hpp                                                          //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_PEARSON_CHISQ_INDEPENDENCE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_PEARSON_CHISQ_INDEPENDENCE_HPP_ER_2010

#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/feature.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/null_hypothesis.hpp>

#endif


#include <benchmark/benchmark.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "UKF/Types.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"

enum MyStateVectorFields {
    AngularVelocity,
    Altitude,
    Velocity,
    Attitude
};

using MyStateVector = UKF::StateVector<
    UKF::Field<Velocity, UKF::Vector<3>>,
    UKF::Field<AngularVelocity, UKF::Vector<3>>,
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<Altitude, real_t>
>;

enum MyFields {
    StaticPressure,
    DynamicPressure,
    Accelerometer,
    Gyroscope
};

using MV_Fixed = UKF::FixedMeasurementVector<
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Gyroscope, UKF::Vector<3>>,
    UKF::Field<StaticPressure, real_t>,
    UKF::Field<DynamicPressure, real_t>
>;

namespace UKF {
template <> template <>
UKF::Vector<3> MV_Fixed::expected_measurement
<MyStateVector, Accelerometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
}

template <> template <>
UKF::Vector<3> MV_Fixed::expected_measurement
<MyStateVector, Gyroscope>(const MyStateVector& state) {
    return state.get_field<AngularVelocity>();
}

template <> template <>
real_t MV_Fixed::expected_measurement
<MyStateVector, StaticPressure>(const MyStateVector& state) {
    return 101.3 - 1.2*(state.get_field<Altitude>() / 100.0);
}

template <> template <>
real_t MV_Fixed::expected_measurement
<MyStateVector, DynamicPressure>(const MyStateVector& state) {
    return 0.5 * 1.225 * state.get_field<Velocity>().squaredNorm();
}

}

using MV_Dynamic = UKF::DynamicMeasurementVector<
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Gyroscope, UKF::Vector<3>>,
    UKF::Field<StaticPressure, real_t>,
    UKF::Field<DynamicPressure, real_t>
>;

namespace UKF {
template <> template <>
UKF::Vector<3> MV_Dynamic::expected_measurement
<MyStateVector, Accelerometer>(const MyStateVector& state) {
    return state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -9.8);
}

template <> template <>
UKF::Vector<3> MV_Dynamic::expected_measurement
<MyStateVector, Gyroscope>(const MyStateVector& state) {
    return state.get_field<AngularVelocity>();
}

template <> template <>
real_t MV_Dynamic::expected_measurement
<MyStateVector, StaticPressure>(const MyStateVector& state) {
    return 101.3 - 1.2*(state.get_field<Altitude>() / 100.0);
}

template <> template <>
real_t MV_Dynamic::expected_measurement
<MyStateVector, DynamicPressure>(const MyStateVector& state) {
    return 0.5 * 1.225 * state.get_field<Velocity>().squaredNorm();
}

}

/*
Tests to compare set/get performance between fixed and dynamic measurement vectors.
*/
void MeasurementVectorFixed_SetGetField(benchmark::State& state) {
    MV_Fixed test_measurement;
    while(state.KeepRunning()) {
        test_measurement.set_field<Accelerometer>(UKF::Vector<3>(1, 2, 3));
        benchmark::DoNotOptimize(test_measurement.get_field<Accelerometer>());
    }
}

BENCHMARK(MeasurementVectorFixed_SetGetField);

void MeasurementVectorDynamic_SetGetField(benchmark::State& state) {
    MV_Dynamic test_measurement;
    while(state.KeepRunning()) {
        test_measurement.set_field<Accelerometer>(UKF::Vector<3>(1, 2, 3));
        benchmark::DoNotOptimize(test_measurement.get_field<Accelerometer>());
    }
}

BENCHMARK(MeasurementVectorDynamic_SetGetField);

void MeasurementVectorFixed_SigmaPointGeneration(benchmark::State& state) {
    MyStateVector test_state;
    MV_Fixed test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);

    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points));
    }
}

BENCHMARK(MeasurementVectorFixed_SigmaPointGeneration);

void MeasurementVectorDynamic_SigmaPointGeneration(benchmark::State& state) {
    MyStateVector test_state;
    MV_Dynamic test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);

    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points));
    }
}

BENCHMARK(MeasurementVectorDynamic_SigmaPointGeneration);

void MeasurementVectorFixed_FullMeasurementCalculation(benchmark::State& state) {
    MyStateVector test_state;
    MV_Fixed test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);

    MV_Fixed::SigmaPointDistribution<MyStateVector> measurement_sigma_points;
    MV_Fixed mean_measurement;
    MV_Fixed::SigmaPointDeltas<MyStateVector> sigma_point_deltas;
    MV_Fixed::CovarianceMatrix calculated_covariance;

    while(state.KeepRunning()) {
        measurement_sigma_points = test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);
        mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
        sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);
        calculated_covariance = mean_measurement.calculate_sigma_point_covariance<MyStateVector>(sigma_point_deltas);
    }
}

BENCHMARK(MeasurementVectorFixed_FullMeasurementCalculation);

void MeasurementVectorDynamic_FullMeasurementCalculation(benchmark::State& state) {
    MyStateVector test_state;
    MV_Dynamic test_measurement;

    test_measurement.set_field<Accelerometer>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<Gyroscope>(UKF::Vector<3>(0, 0, 0));
    test_measurement.set_field<StaticPressure>(0);
    test_measurement.set_field<DynamicPressure>(0);

    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(1000);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution(covariance);

    MV_Dynamic::SigmaPointDistribution<MyStateVector> measurement_sigma_points(
        test_measurement.size(), MyStateVector::num_sigma());
    MV_Dynamic mean_measurement(test_measurement.size());
    MV_Dynamic::SigmaPointDeltas<MyStateVector> sigma_point_deltas(test_measurement.size(), MyStateVector::num_sigma());
    MV_Dynamic::CovarianceMatrix calculated_covariance(test_measurement.size(), test_measurement.size());

    while(state.KeepRunning()) {
        measurement_sigma_points = test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);
        mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
        sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);
        calculated_covariance = mean_measurement.calculate_sigma_point_covariance<MyStateVector>(sigma_point_deltas);
    }
}

BENCHMARK(MeasurementVectorDynamic_FullMeasurementCalculation);

void MeasurementVectorFixed_MeasurementCovariance(benchmark::State& state) {
    MV_Fixed test_measurement, expected_measurement;
    MV_Fixed::CovarianceVector measurement_covariance;

    measurement_covariance.set_field<Accelerometer>(UKF::Vector<3>(1, 2, 3));
    measurement_covariance.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));
    measurement_covariance.set_field<StaticPressure>(7);
    measurement_covariance.set_field<DynamicPressure>(8);

    expected_measurement.set_field<Accelerometer>(UKF::Vector<3>(1, 2, 3));
    expected_measurement.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));
    expected_measurement.set_field<StaticPressure>(7);
    expected_measurement.set_field<DynamicPressure>(8);

    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(test_measurement.calculate_measurement_covariance(
            measurement_covariance, expected_measurement));
    }
}

BENCHMARK(MeasurementVectorFixed_MeasurementCovariance);

void MeasurementVectorDynamic_MeasurementCovariance(benchmark::State& state) {
    MV_Dynamic test_measurement, expected_measurement;
    MV_Dynamic::CovarianceVector measurement_covariance;

    measurement_covariance.set_field<Accelerometer>(UKF::Vector<3>(1, 2, 3));
    measurement_covariance.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));
    measurement_covariance.set_field<StaticPressure>(7);
    measurement_covariance.set_field<DynamicPressure>(8);

    expected_measurement.set_field<Accelerometer>(UKF::Vector<3>(1, 2, 3));
    expected_measurement.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));
    expected_measurement.set_field<StaticPressure>(7);
    expected_measurement.set_field<DynamicPressure>(8);

    while(state.KeepRunning()) {
        benchmark::DoNotOptimize(test_measurement.calculate_measurement_covariance(
            measurement_covariance, expected_measurement));
    }
}

BENCHMARK(MeasurementVectorDynamic_MeasurementCovariance);


/*    Copyright (c) 2010-2019, 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_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>
#include <Eigen/Core>

#include <boost/test/unit_test.hpp>

#include "tudat/astro/basic_astro/timeConversions.h"
#include "tudat/interface/sofa/sofaTimeConversions.h"
#include "tudat/basics/timeType.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::sofa_interface;
using namespace tudat::basic_astrodynamics;

BOOST_AUTO_TEST_SUITE( test_sofa_time_conversions )

//! Function to test Sofa time conversion functions (except TT<->TDB) for templated time scalar type.
template< typename ScalarType >
void testTimeConversions( )
{
    // Define UTC date and time.
    int year = 2006;
    int months = 1;
    int days = 15;
    int hours = 21;
    int minutes = 24;
    ScalarType seconds = static_cast< ScalarType >( 37.5 );

    // Get UTC time.
    ScalarType utcSecondsSinceJ2000 =
            convertCalendarDateToJulianDaysSinceEpoch< ScalarType >(
                year, months, days, hours, minutes, seconds,
                getJulianDayOnJ2000< ScalarType >( ) ) *
            physical_constants::getJulianDay< ScalarType >( );

    // Convert to TAI time.
    ScalarType taiSecondsSinceJ2000 = convertUTCtoTAI( utcSecondsSinceJ2000 );

    // Get number of leap seconds and test
    ScalarType numberOfLeapSeconds = static_cast< ScalarType >( 33 );
    BOOST_CHECK_CLOSE_FRACTION( taiSecondsSinceJ2000 - utcSecondsSinceJ2000, numberOfLeapSeconds,
                                utcSecondsSinceJ2000 * std::numeric_limits< ScalarType >::epsilon( ) );

    // Recover UTC and test
    ScalarType recoveredUtcSecondsSinceJ2000 = convertTAItoUTC( taiSecondsSinceJ2000 );
    BOOST_CHECK_SMALL( recoveredUtcSecondsSinceJ2000 - utcSecondsSinceJ2000,
                       std::numeric_limits< ScalarType >::epsilon( ) );

    // Get TT time and compare against expected result.
    ScalarType ttSecondsSinceJ2000 = convertUTCtoTT( utcSecondsSinceJ2000 );
    ScalarType ttMinusTai = static_cast< ScalarType >( 32.184 );
    BOOST_CHECK_CLOSE_FRACTION( ttSecondsSinceJ2000 - utcSecondsSinceJ2000,
                                numberOfLeapSeconds + ttMinusTai,
                                utcSecondsSinceJ2000 * std::numeric_limits< ScalarType >::epsilon( ) );

    // Recover UTC from TT and compare against original UTC.
    recoveredUtcSecondsSinceJ2000 = convertTTtoUTC( ttSecondsSinceJ2000 );
    BOOST_CHECK_SMALL( recoveredUtcSecondsSinceJ2000 - utcSecondsSinceJ2000,
                       std::numeric_limits< ScalarType >::epsilon( ) );

}

//! Test TDB<->TT conversions using SOFA cookbook data
BOOST_AUTO_TEST_CASE( testSofaTimeConversions )
{

    // Test conversions for double/long double
    testTimeConversions< double >( );
    testTimeConversions< long double >( );

    // Test TDB - TT calculation, using code from SOFA Time Conversions cookbook.
    {
        // Calculate UTC time from date/time
        int year = 2006;
        int months = 1;
        int days = 15;
        int hours = 21;
        int minutes = 24;
        double seconds = 37.5;
        double utcSecondsSinceJ2000 =
                convertCalendarDateToJulianDaysSinceEpoch< double >(
                    year, months, days, hours, minutes, seconds, getJulianDayOnJ2000< double >( ) ) *
                physical_constants::getJulianDay< double >( );

        // Get UT1 time
        double dut = 0.3341;
        double ut1SecondsSinceEpoch = utcSecondsSinceJ2000 + dut;

        // Convert UTC to TT
        double ttSecondsSinceJ2000 = convertUTCtoTT( utcSecondsSinceJ2000 );

        int latnd, latnm, lonwd, lonwm, iy, mo, id, ih, im;
        double slatn, slonw, hm, elon, phi, xyz[3], u, v, sec,
                utc1, utc2, ut11, ut12, ut, tai1, tai2, tt1, tt2,
                tcg1, tcg2;

        // Run code from Sofa cookbook to obtain TDB - TT (and ground station position).
        Eigen::Vector3d referencePoint;
        double dtr;
        {
            /* UTC date and time. */
            iy = 2006;
            mo = 1;
            id = 15;
            ih = 21;
            im = 24;
            sec = 37.5;
            /* Transform into internal format. */
            iauDtf2d ( "UTC", iy, mo, id, ih, im, sec, &utc1, &utc2 );

            /* UTC -> UT1. */
            iauUtcut1 ( utc1, utc2, dut, &ut11, &ut12 );

            /* Extract fraction for TDB-TT calculation, later. */
            ut = fmod ( fmod(ut11,1.0) + fmod(ut12,1.0), 1.0 ) + 0.5;
            /* UTC -> TAI -> TT -> TCG. */
            iauUtctai ( utc1, utc2, &tai1, &tai2 );

            iauTaitt ( tai1, tai2, &tt1, &tt2 );

            iauTttcg ( tt1, tt2, &tcg1, &tcg2 );

            /* Site terrestrial coordinates (WGS84). */
            latnd = 19;
            latnm = 28;
            slatn = 52.5;
            lonwd = 155;
            lonwm = 55;
            slonw = 59.6;
            hm = 0.0;

            /* Transform to geocentric. */
            iauAf2a ( '+', latnd, latnm, slatn, &phi );
            iauAf2a ( '-', lonwd, lonwm, slonw, &elon );
            iauGd2gc ( 1, elon, phi, hm, xyz );
            u = sqrt ( xyz[0]*xyz[0] + xyz[1]*xyz[1] );
            v = xyz[2];

            /* UTC date and time. */
            iy = 2006;
            mo = 1;
            id = 15;
            ih = 21;
            im = 24;
            sec = 37.5;

            /* Transform into internal format. */
            iauDtf2d ( "UTC", iy, mo, id, ih, im, sec, &utc1, &utc2 );

            /* UT1-UTC (s, from IERS). */
            dut = 0.3341;

            /* UTC -> UT1. */
            iauUtcut1 ( utc1, utc2, dut, &ut11, &ut12 );

            /* Extract fraction for TDB-TT calculation, later. */
            ut = fmod ( fmod(ut11,1.0) + fmod(ut12,1.0), 1.0 ) + 0.5;

            /* UTC -> TAI -> TT -> TCG. */
            iauUtctai ( utc1, utc2, &tai1, &tai2 );
            iauTaitt ( tai1, tai2, &tt1, &tt2 );
            iauTttcg ( tt1, tt2, &tcg1, &tcg2 );

            /* TDB-TT (using TT as a substitute for TDB). */
            dtr = iauDtdb ( tt1, tt2, ut, elon, u/1e3, v/1e3 );

            // Define reference point and
            referencePoint = ( Eigen::Vector3d( ) << xyz[ 0 ], xyz[ 1 ], xyz[ 2 ] ).finished( );
        }

        // Calculate UT1 fraction of day
        double utFractionOfDay = std::fmod( ut1SecondsSinceEpoch / physical_constants::JULIAN_DAY + 0.5, 1.0 );

        // Calculate TDB - TT from Sofa and comapre against cookbook result.
        double tdbMinusTt = getTDBminusTT( ttSecondsSinceJ2000, utFractionOfDay, referencePoint );
        BOOST_CHECK_SMALL( tdbMinusTt - dtr, std::numeric_limits< double >::epsilon( ) );

        // Check validity of using TT as subsititute for TDB in conversions
        double dtr2 = iauDtdb ( tt1, tt2 + dtr / physical_constants::JULIAN_DAY, ut, elon, u/1.0e3, v/1.0e3 );
        double dtr3 = iauDtdb ( tt1, tt2 + dtr2 / physical_constants::JULIAN_DAY, ut, elon, u/1.0e3, v/1.0e3 );
        BOOST_CHECK_SMALL( dtr - dtr2, 1.0E-12 );
        BOOST_CHECK_SMALL( dtr2 - dtr3, 1.0E-15 );

        // Check approximate conversion, omitting utc-ut1 correction.
        double tdbMinusTtApproximate = getTDBminusTT( ttSecondsSinceJ2000, referencePoint );
        BOOST_CHECK_SMALL( tdbMinusTtApproximate - tdbMinusTt, 1.0E-10 );    }

}

BOOST_AUTO_TEST_CASE( testLeapSecondIdentification )
{
    Eigen::Matrix< int, Eigen::Dynamic, 3 > leapSecondDays;
    leapSecondDays.resize( 27, 3 );
    leapSecondDays << 1, 7, 1972,
            1, 1, 1973,
            1, 1, 1974,
            1, 1, 1975,
            1, 1, 1976,
            1, 1, 1977,
            1, 1, 1978,
            1, 1, 1979,
            1, 1, 1980,
            1, 7, 1981,
            1, 7, 1982,
            1, 7, 1983,
            1, 7, 1985,
            1, 1, 1988,
            1, 1, 1990,
            1, 1, 1991,
            1, 7, 1992,
            1, 7, 1993,
            1, 7, 1994,
            1, 1, 1996,
            1, 7, 1997,
            1, 1, 1999,
            1, 1, 2006,
            1, 1, 2009,
            1, 7, 2012,
            1, 7, 2015,
            1, 1, 2017;

    for( unsigned int i = 0; i < leapSecondDays.rows( ); i++ )
    {
        double utcTimeOfLeapSeconds = basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                    leapSecondDays( i, 2 ), leapSecondDays( i, 1 ), leapSecondDays( i, 0 ), 0, 0, 0.0,
                    basic_astrodynamics::JULIAN_DAY_ON_J2000 );
        BOOST_CHECK_EQUAL( sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds - 1.0E-6 ) -
                           sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds + 1.0E-6 ), -1.0 );
        BOOST_CHECK_EQUAL( sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds + 1.0E-6 ) -
                           sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds + 2.0E-6 ), 0.0 );

        double taiPreLeap = tudat::sofa_interface::convertUTCtoTAI< double >(
                    utcTimeOfLeapSeconds * physical_constants::JULIAN_DAY - 1.0E-6 );
        double taiPostLeap = tudat::sofa_interface::convertUTCtoTAI< double >(
                    utcTimeOfLeapSeconds * physical_constants::JULIAN_DAY + 1.0E-6 );
        BOOST_CHECK_SMALL(  std::fabs( taiPostLeap - taiPreLeap - ( 1.0 + 2.0E-6 ) ), 1.0E-7 );

        double utcPreLeap = tudat::sofa_interface::convertTAItoUTC< double >( taiPreLeap );
        double utcPostLeap = tudat::sofa_interface::convertTAItoUTC< double >( taiPostLeap );

        BOOST_CHECK_SMALL( std::fabs( utcPostLeap - utcPreLeap - ( 2.0E-6 ) ), 1.0E-7 );
    }

    for( unsigned int i = 0; i < leapSecondDays.rows( ); i++ )
    {
        Time utcTimeOfLeapSeconds = basic_astrodynamics::convertCalendarDateToJulianDaysSinceEpoch(
                    leapSecondDays( i, 2 ), leapSecondDays( i, 1 ), leapSecondDays( i, 0 ), 0, 0, 0.0,
                    basic_astrodynamics::JULIAN_DAY_ON_J2000 );
        BOOST_CHECK_EQUAL( sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds - 1.0E-6 ) -
                           sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds + 1.0E-6 ), -1.0 );
        BOOST_CHECK_EQUAL( sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds + 1.0E-6 ) -
                           sofa_interface::getDeltaAtFromUtc( utcTimeOfLeapSeconds + 2.0E-6 ), 0.0 );

        Time taiPreLeap = tudat::sofa_interface::convertUTCtoTAI< Time >(
                    utcTimeOfLeapSeconds * physical_constants::JULIAN_DAY - 1.0E-6 );
        Time taiPostLeap = tudat::sofa_interface::convertUTCtoTAI< Time >(
                    utcTimeOfLeapSeconds * physical_constants::JULIAN_DAY + 1.0E-6 );

        long double timeDifferenceTai = static_cast< long double >( taiPostLeap - taiPreLeap ) - ( 1.0L + 2.0E-6 );
        BOOST_CHECK_SMALL( std::fabs( timeDifferenceTai ), 3600.0L * std::numeric_limits< long double >::epsilon( ) );

        Time utcPreLeap = tudat::sofa_interface::convertTAItoUTC< Time >( taiPreLeap );
        Time utcPostLeap = tudat::sofa_interface::convertTAItoUTC< Time >( taiPostLeap );

        long double timeDifferenceUtc = static_cast< long double >( utcPostLeap - utcPreLeap ) - ( 2.0E-6 );
        BOOST_CHECK_SMALL( std::fabs( timeDifferenceUtc ), 3600.0L * std::numeric_limits< long double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <frovedis.hpp>
#include <frovedis/matrix/dense_eigen.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

/*
  input: m x n
  u: m x k (<-m)
  s: k(<-m) x k(<-n)
  vt: k(<-n) x n / v: n x k
 */
void do_dense_eigen_sym(const string& input_matrix, const string& d_file,
                         const string& v_file, string mode, int k,
                         bool binary) {
  time_spent t(DEBUG);
  rowmajor_matrix<double> matrix;
  if(binary) {
    matrix = make_rowmajor_matrix_loadbinary<double>(input_matrix);
  } else {
    matrix = make_rowmajor_matrix_load<double>(input_matrix);
  }
  t.show("load time: ");
  colmajor_matrix<double> v;
  diag_matrix_local<double> d;
  time_spent t2(DEBUG), t3(DEBUG);
  dense_eigen_sym<double>(matrix, d, v, mode, k);
  t2.show("dense_eigen_sym: ");
  t3.show("total time w/o I/O: ");
  if(binary) {
    d.savebinary(d_file);
    v.to_rowmajor().savebinary(v_file);
  } else {
    d.save(d_file);
    v.to_rowmajor().save(v_file);
  }
  t2.show("save time: ");
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("input,i", value<string>(), "input dense data matrix")
    ("d,d", value<string>(), "eigen values to save")
    ("v,v", value<string>(), "eigen vectors to save")
    ("k,k", value<int>(), "number of eigen values to compute")
    ("mode", value<string>(), "SM: from small, LM: from large, etc. [default: SM]")
    ("verbose", "set loglevel DEBUG")
    ("verbose2", "set loglevel TRACE")
    ("binary,b", "use binary input/output");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  string input, d, v;
  int k;
  bool binary = false;

  string mode = "SM";

  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<string>();
  } else {
    cerr << "input is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("d")){
    d = argmap["d"].as<string>();
  } else {
    cerr << "file to store eigen value is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("v")){
    v = argmap["v"].as<string>();
  } else {
    cerr << "file to store eigen vector is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("mode")){
    mode = argmap["mode"].as<string>();
  }

  if(argmap.count("k")){
    k = argmap["k"].as<int>();
  } else {
    cerr << "number of eigen values to compute is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("binary")){
    binary = true;
  }

  if(argmap.count("verbose")){
    set_loglevel(DEBUG);
  }

  if(argmap.count("verbose2")){
    set_loglevel(TRACE);
  }

  do_dense_eigen_sym(input, d, v, mode, k, binary);
}


#include <NTL/ZZ.h>
#include <NTL/vec_ZZ.h>
#include <NTL/Lazy.h>
#include <NTL/fileio.h>

NTL_START_IMPL

const ZZ& ZZ::zero()
{
   NTL_THREAD_LOCAL static ZZ z;
   return z;
}

const ZZ& ZZ_expo(long e)
{
   NTL_THREAD_LOCAL static ZZ expo_helper;
   conv(expo_helper, e);
   return expo_helper;
}

void AddMod(ZZ& x, const ZZ& a, long b, const ZZ& n)
{
   NTL_ZZRegister(B);
   conv(B, b);
   AddMod(x, a, B, n);
}

void SubMod(ZZ& x, const ZZ& a, long b, const ZZ& n)
{
   NTL_ZZRegister(B);
   conv(B, b);
   SubMod(x, a, B, n);
}

void SubMod(ZZ& x, long a, const ZZ& b, const ZZ& n)
{
   NTL_ZZRegister(A);
   conv(A, a);
   SubMod(x, A, b, n);
}

// ****** input and output

NTL_THREAD_LOCAL static long iodigits = 0;
NTL_THREAD_LOCAL static long ioradix = 0;

// iodigits is the greatest integer such that 10^{iodigits} < NTL_WSP_BOUND
// ioradix = 10^{iodigits}

static void InitZZIO()
{
   long x;

   x = (NTL_WSP_BOUND-1)/10;
   iodigits = 0;
   ioradix = 1;

   while (x) {
      x = x / 10;
      iodigits++;
      ioradix = ioradix * 10;
   }

   if (iodigits <= 0) TerminalError("problem with I/O");
}

istream& operator>>(istream& s, ZZ& x)
{
   long c;
   long cval;
   long sign;
   long ndigits;
   long acc;
   NTL_ZZRegister(a);

   if (!s) NTL_INPUT_ERROR(s, "bad ZZ input");

   if (!iodigits) InitZZIO();

   a = 0;

   SkipWhiteSpace(s);
   c = s.peek();

   if (c == '-') {
      sign = -1;
      s.get();
      c = s.peek();
   }
   else
      sign = 1;

   cval = CharToIntVal(c);

   if (cval < 0 || cval > 9) NTL_INPUT_ERROR(s, "bad ZZ input");

   ndigits = 0;
   acc = 0;
   while (cval >= 0 && cval <= 9) {
      acc = acc*10 + cval;
      ndigits++;

      if (ndigits == iodigits) {
         mul(a, a, ioradix);
         add(a, a, acc);
         ndigits = 0;
         acc = 0;
      }

      s.get();
      c = s.peek();
      cval = CharToIntVal(c);
   }

   if (ndigits != 0) {
      long mpy = 1;
      while (ndigits > 0) {
         mpy = mpy * 10;
         ndigits--;
      }

      mul(a, a, mpy);
      add(a, a, acc);
   }

   if (sign == -1)
      negate(a, a);

   x = a;
   return s;
}

// The class _ZZ_local_stack should be defined in an empty namespace,
// but since I don't want to rely on namespaces, we just give it a funny
// name to avoid accidental name clashes.

struct _ZZ_local_stack {
   long top;
   Vec<long> data;

   _ZZ_local_stack() { top = -1; }

   long pop() { return data[top--]; }
   long empty() { return (top == -1); }
   void push(long x);
};

void _ZZ_local_stack::push(long x)
{
   if (top+1 >= data.length())
      data.SetLength(max(32, long(1.414*data.length())));

   top++;
   data[top] = x;
}

static
void PrintDigits(ostream& s, long d, long justify)
{
   NTL_THREAD_LOCAL static Vec<char> buf(INIT_SIZE, iodigits);

   long i = 0;

   while (d) {
      buf[i] = IntValToChar(d % 10);
      d = d / 10;
      i++;
   }

   if (justify) {
      long j = iodigits - i;
      while (j > 0) {
         s << "0";
         j--;
      }
   }

   while (i > 0) {
      i--;
      s << buf[i];
   }
}

ostream& operator<<(ostream& s, const ZZ& a)
{
   ZZ b;
   _ZZ_local_stack S;
   long r;
   long k;

   if (!iodigits) InitZZIO();

   b = a;

   k = sign(b);

   if (k == 0) {
      s << "0";
      return s;
   }

   if (k < 0) {
      s << "-";
      negate(b, b);
   }

   do {
      r = DivRem(b, b, ioradix);
      S.push(r);
   } while (!IsZero(b));

   r = S.pop();
   PrintDigits(s, r, 0);

   while (!S.empty()) {
      r = S.pop();
      PrintDigits(s, r, 1);
   }

   return s;
}

long GCD(long a, long b)
{
   long u, v, t, x;

   if (a < 0) {
      if (a < -NTL_MAX_LONG) ResourceError("GCD: integer overflow");
      a = -a;
   }

   if (b < 0) {
      if (b < -NTL_MAX_LONG) ResourceError("GCD: integer overflow");
      b = -b;
   }

   if (b==0)
      x = a;
   else {
      u = a;
      v = b;
      do {
         t = u % v;
         u = v;
         v = t;
      } while (v != 0);

      x = u;
   }

   return x;
}

void XGCD(long& d, long& s, long& t, long a, long b)
{
   long  u, v, u0, v0, u1, v1, u2, v2, q, r;

   long aneg = 0, bneg = 0;

   if (a < 0) {
      if (a < -NTL_MAX_LONG) ResourceError("XGCD: integer overflow");
      a = -a;
      aneg = 1;
   }

   if (b < 0) {
      if (b < -NTL_MAX_LONG) ResourceError("XGCD: integer overflow");
      b = -b;
      bneg = 1;
   }

   u1=1; v1=0;
   u2=0; v2=1;
   u = a; v = b;

   while (v != 0) {
      q = u / v;
      r = u % v;
      u = v;
      v = r;
      u0 = u2;
      v0 = v2;
      u2 =  u1 - q*u2;
      v2 = v1- q*v2;
      u1 = u0;
      v1 = v0;
   }

   if (aneg)
      u1 = -u1;

   if (bneg)
      v1 = -v1;

   d = u;
   s = u1;
   t = v1;
}

long InvMod(long a, long n)
{
   long d, s, t;

   XGCD(d, s, t, a, n);
   if (d != 1) InvModError("InvMod: inverse undefined");
   if (s < 0)
      return s + n;
   else
      return s;
}

long PowerMod(long a, long ee, long n)
{
   long x, y;

   unsigned long e;

   if (ee < 0)
      e = - ((unsigned long) ee);
   else
      e = ee;

   x = 1;
   y = a;
   while (e) {
      if (e & 1) x = MulMod(x, y, n);
      y = MulMod(y, y, n);
      e = e >> 1;
   }

   if (ee < 0) x = InvMod(x, n);

   return x;
}

long ProbPrime(long n, long NumTests)
{
   long m, x, y, z;
   long i, j, k;

   if (n <= 1) return 0;

   if (n == 2) return 1;
   if (n % 2 == 0) return 0;

   if (n == 3) return 1;
   if (n % 3 == 0) return 0;

   if (n == 5) return 1;
   if (n % 5 == 0) return 0;

   if (n == 7) return 1;
   if (n % 7 == 0) return 0;

   if (n >= NTL_SP_BOUND) {
      return ProbPrime(to_ZZ(n), NumTests);
   }

   m = n - 1;
   k = 0;
   while((m & 1) == 0) {
      m = m >> 1;
      k++;
   }

   // n - 1 == 2^k * m, m odd

   for (i = 0; i < NumTests; i++) {
      do {
         x = RandomBnd(n);
      } while (x == 0);
      // x == 0 is not a useful candidtae for a witness!

      if (x == 0) continue;
      z = PowerMod(x, m, n);
      if (z == 1) continue;

      j = 0;
      do {
         y = z;
         z = MulMod(y, y, n);
         j++;
      } while (j != k && z != 1);

      if (z != 1 || y !=  n-1) return 0;
   }

   return 1;
}

long MillerWitness(const ZZ& n, const ZZ& x)
{
   ZZ m, y, z;
   long j, k;

   if (x == 0) return 0;

   add(m, n, -1);
   k = MakeOdd(m);
   // n - 1 == 2^k * m, m odd

   PowerMod(z, x, m, n);
   if (z == 1) return 0;

   j = 0;
   do {
      y = z;
      SqrMod(z, y, n);
      j++;
   } while (j != k && z != 1);

   if (z != 1) return 1;
   add(y, y, 1);
   if (y != n) return 1;
   return 0;
}

// ComputePrimeBound computes a reasonable bound for trial
// division in the Miller-Rabin test.
// It is computed a bit on the "low" side, since being a bit
// low doesn't hurt much, but being too high can hurt a lot.

static
long ComputePrimeBound(long bn)
{
   long wn = (bn+NTL_ZZ_NBITS-1)/NTL_ZZ_NBITS;

   long fn;

   if (wn <= 36)
      fn = wn/4 + 1;
   else
      fn = long(1.67*sqrt(double(wn)));

   long prime_bnd;

   if (NumBits(bn) + NumBits(fn) > NTL_SP_NBITS)
      prime_bnd = NTL_SP_BOUND;
   else
      prime_bnd = bn*fn;

   return prime_bnd;
}

long ProbPrime(const ZZ& n, long NumTrials)
{
   if (n <= 1) return 0;

   if (n.SinglePrecision()) {
      return ProbPrime(to_long(n), NumTrials);
   }

   long prime_bnd = ComputePrimeBound(NumBits(n));

   PrimeSeq s;
   long p;

   p = s.next();
   while (p && p < prime_bnd) {
      if (rem(n, p) == 0)
         return 0;

      p = s.next();
   }

   ZZ W;
   W = 2;

   // first try W == 2....the exponentiation
   // algorithm runs slightly faster in this case

   if (MillerWitness(n, W))
      return 0;

   long i;

   for (i = 0; i < NumTrials; i++) {
      do {
         RandomBnd(W, n);
      } while (W == 0);
      // W == 0 is not a useful candidate for a witness!

      if (MillerWitness(n, W))
         return 0;
   }

   return 1;
}

void RandomPrime(ZZ& n, long l, long NumTrials)
{
   if (l <= 1)
      LogicError("RandomPrime: l out of range");

   if (l == 2) {
      if (RandomBnd(2))
         n = 3;
      else
         n = 2;

      return;
   }

   do {
      RandomLen(n, l);
      if (!IsOdd(n)) add(n, n, 1);
   } while (!ProbPrime(n, NumTrials));
}

void NextPrime(ZZ& n, const ZZ& m, long NumTrials)
{
   ZZ x;

   if (m <= 2) {
      n = 2;
      return;
   }

   x = m;

   while (!ProbPrime(x, NumTrials))
      add(x, x, 1);

   n = x;
}

long NextPrime(long m, long NumTrials)
{
   long x;

   if (m <= 2)
      return 2;

   x = m;

   while (x < NTL_SP_BOUND && !ProbPrime(x, NumTrials))
      x++;

   if (x >= NTL_SP_BOUND)
      ResourceError("NextPrime: no more primes");

   return x;
}

long NextPowerOfTwo(long m)
{
   long k;
   unsigned long n, um;

   if (m < 0) return 0;

   um = m;
   n = 1;
   k = 0;

   while (n < um) {
      n = n << 1;
      k++;
   }

   if (k >= NTL_BITS_PER_LONG-1)
      ResourceError("NextPowerOfTwo: overflow");

   return k;
}

long NumBits(long a)
{
   unsigned long aa;
   if (a < 0)
      aa = - ((unsigned long) a);
   else
      aa = a;

   long k = 0;
   while (aa) {
      k++;
      aa = aa >> 1;
   }

   return k;
}

long bit(long a, long k)
{
   unsigned long aa;
   if (a < 0)
      aa = - ((unsigned long) a);
   else
      aa = a;

   if (k < 0 || k >= NTL_BITS_PER_LONG)
      return 0;
   else
      return long((aa >> k) & 1);
}

long divide(ZZ& q, const ZZ& a, const ZZ& b)
{
   NTL_ZZRegister(qq);
   NTL_ZZRegister(r);

   if (IsZero(b)) {
      if (IsZero(a)) {
         clear(q);
         return 1;
      }
      else
         return 0;
   }

   if (IsOne(b)) {
      q = a;
      return 1;
   }

   DivRem(qq, r, a, b);
   if (!IsZero(r)) return 0;
   q = qq;
   return 1;
}

long divide(const ZZ& a, const ZZ& b)
{
   NTL_ZZRegister(r);

   if (IsZero(b)) return IsZero(a);
   if (IsOne(b)) return 1;

   rem(r, a, b);
   return IsZero(r);
}

long divide(ZZ& q, const ZZ& a, long b)
{
   NTL_ZZRegister(qq);

   if (!b) {
      if (IsZero(a)) {
         clear(q);
         return 1;
      }
      else
         return 0;
   }

   if (b == 1) {
      q = a;
      return 1;
   }

   long r = DivRem(qq, a, b);
   if (r) return 0;
   q = qq;
   return 1;
}

long divide(const ZZ& a, long b)
{
   if (!b) return IsZero(a);
   if (b == 1) {
      return 1;
   }

   long r = rem(a,  b);
   return (r == 0);
}

void InvMod(ZZ& x, const ZZ& a, const ZZ& n)
{
   // NOTE: the underlying LIP routines write to the first argument,
   // even if inverse is undefined

   NTL_ZZRegister(xx);
   if (InvModStatus(xx, a, n))
      InvModError("InvMod: inverse undefined", a, n);
   x = xx;
}

void PowerMod(ZZ& x, const ZZ& a, const ZZ& e, const ZZ& n)
{
   // NOTE: this ensures that all modular inverses are computed
   // in the routine InvMod above, rather than the LIP-internal
   // modular inverse routine
   if (e < 0) {
      ZZ a_inv;
      ZZ e_neg;

      InvMod(a_inv, a, n);
      negate(e_neg, e);
      LowLevelPowerMod(x, a_inv, e_neg, n);
   }
   else
      LowLevelPowerMod(x, a, e, n);
}

#ifdef NTL_EXCEPTIONS

void InvModError(const char *s, const ZZ& a, const ZZ& n)
{
   throw InvModErrorObject(s, a, n);
}

#else

void InvModError(const char *s, const ZZ& a, const ZZ& n)
{
   TerminalError(s);
}

#endif

long RandomPrime_long(long l, long NumTrials)
{
   if (l <= 1 || l >= NTL_BITS_PER_LONG)
      ResourceError("RandomPrime: length out of range");

   long n;
   do {
      n = RandomLen_long(l);
   } while (!ProbPrime(n, NumTrials));

   return n;
}

static Lazy< Vec<char> > lowsieve_storage;
// This is a GLOBAL VARIABLE

PrimeSeq::PrimeSeq()
{
   movesieve = 0;
   pshift = -1;
   pindex = -1;
   exhausted = 0;
}

long PrimeSeq::next()
{
   if (exhausted) {
      return 0;
   }

   if (pshift < 0) {
      shift(0);
      return 2;
   }

   for (;;) {
      const char *p = movesieve;
      long i = pindex;

      while ((++i) < NTL_PRIME_BND) {
         if (p[i]) {
            pindex = i;
            return pshift + 2 * i + 3;
         }
      }

      long newshift = pshift + 2*NTL_PRIME_BND;

      if (newshift > 2 * NTL_PRIME_BND * (2 * NTL_PRIME_BND + 1)) {
         /* end of the road */
         exhausted = 1;
         return 0;
      }

      shift(newshift);
   }
}

void PrimeSeq::shift(long newshift)
{
   long i;
   long j;
   long jstep;
   long jstart;
   long ibound;
   char *p;

   if (!lowsieve_storage.built())
      start();

   const char *lowsieve = lowsieve_storage->elts();

   if (newshift < 0) {
      pshift = -1;
   }
   else if (newshift == 0) {
      pshift = 0;
      movesieve = lowsieve;
   }
   else if (newshift != pshift) {
      if (movesieve_mem.length() == 0) {
         movesieve_mem.SetLength(NTL_PRIME_BND);
      }

      pshift = newshift;
      movesieve = p = movesieve_mem.elts();
      for (i = 0; i < NTL_PRIME_BND; i++)
         p[i] = 1;

      jstep = 3;
      ibound = pshift + 2 * NTL_PRIME_BND + 1;
      for (i = 0; jstep * jstep <= ibound; i++) {
         if (lowsieve[i]) {
            if (!((jstart = (pshift + 2) / jstep + 1) & 1))
               jstart++;
            if (jstart <= jstep)
               jstart = jstep;
            jstart = (jstart * jstep - pshift - 3) / 2;
            for (j = jstart; j < NTL_PRIME_BND; j += jstep)
               p[j] = 0;
         }
         jstep += 2;
      }
   }

   pindex = -1;
   exhausted = 0;
}

void PrimeSeq::start()
{
   long i;
   long j;
   long jstep;
   long jstart;
   long ibnd;
   char *p;

   do {
      Lazy< Vec<char> >::Builder builder(lowsieve_storage);
      if (!builder()) break;

      UniquePtr< Vec<char> > ptr;
      ptr.make();
      ptr->SetLength(NTL_PRIME_BND);

      p = ptr->elts();

      for (i = 0; i < NTL_PRIME_BND; i++)
         p[i] = 1;

      jstep = 1;
      jstart = -1;
      ibnd = (SqrRoot(2 * NTL_PRIME_BND + 1) - 3) / 2;
      for (i = 0; i <= ibnd; i++) {
         jstart += 2 * ((jstep += 2) - 1);
         if (p[i])
            for (j = jstart; j < NTL_PRIME_BND; j += jstep)
               p[j] = 0;
      }

      builder.move(ptr);
   } while (0);

}

void PrimeSeq::reset(long b)
{
   if (b > (2*NTL_PRIME_BND+1)*(2*NTL_PRIME_BND+1)) {
      exhausted = 1;
      return;
   }

   if (b <= 2) {
      shift(-1);
      return;
   }

   if ((b & 1) == 0) b++;

   shift(((b-3) / (2*NTL_PRIME_BND))* (2*NTL_PRIME_BND));
   pindex = (b - pshift - 3)/2 - 1;
}

long Jacobi(const ZZ& aa, const ZZ& nn)
{
   ZZ a, n;
   long t, k;
   long d;

   a = aa;
   n = nn;
   t = 1;

   while (a != 0) {
      k = MakeOdd(a);
      d = trunc_long(n, 3);
      if ((k & 1) && (d == 3 || d == 5)) t = -t;

      if (trunc_long(a, 2) == 3 && (d & 3) == 3) t = -t;
      swap(a, n);
      rem(a, a, n);
   }

   if (n == 1)
      return t;
   else
      return 0;
}

void SqrRootMod(ZZ& x, const ZZ& aa, const ZZ& nn)
{
   if (aa == 0 || aa == 1) {
      x = aa;
      return;
   }

   // at this point, we must have nn >= 5

   if (trunc_long(nn, 2) == 3) {  // special case, n = 3 (mod 4)
      ZZ n, a, e, z;

      n = nn;
      a  = aa;

      add(e, n, 1);
      RightShift(e, e, 2);

      PowerMod(z, a, e, n);
      x = z;

      return;
   }

   ZZ n, m;
   int h, nlen;

   n = nn;
   nlen = NumBits(n);

   sub(m, n, 1);
   h = MakeOdd(m);  // h >= 2

   if (nlen > 50 && h < SqrRoot(nlen)) {
      long i, j;
      ZZ a, b, a_inv, c, r, m1, d;

      a = aa;
      InvMod(a_inv, a, n);

      if (h == 2)
         b = 2;
      else {
         do {
            RandomBnd(b, n);
         } while (Jacobi(b, n) != -1);
      }

      PowerMod(c, b, m, n);

      add(m1, m, 1);
      RightShift(m1, m1, 1);
      PowerMod(r, a, m1, n);

      for (i = h-2; i >= 0; i--) {
         SqrMod(d, r, n);
         MulMod(d, d, a_inv, n);
         for (j = 0; j < i; j++)
            SqrMod(d, d, n);
         if (!IsOne(d))
            MulMod(r, r, c, n);
         SqrMod(c, c, n);
      }

      x = r;
      return;
   }

   long i, k;
   ZZ ma, t, u, v, e;
   ZZ t1, t2, t3, t4;

   n = nn;
   NegateMod(ma, aa, n);

   // find t such that t^2 - 4*a is not a square

   MulMod(t1, ma, 4, n);
   do {
      RandomBnd(t, n);
      SqrMod(t2, t, n);
      AddMod(t2, t2, t1, n);
   } while (Jacobi(t2, n) != -1);

   // compute u*X + v = X^{(n+1)/2} mod f, where f = X^2 - t*X + a

   add(e, n, 1);
   RightShift(e, e, 1);

   u = 0;
   v = 1;

   k = NumBits(e);

   for (i = k - 1; i >= 0; i--) {
      add(t2, u, v);
      sqr(t3, t2);  // t3 = (u+v)^2
      sqr(t1, u);
      sqr(t2, v);
      sub(t3, t3, t1);
      sub(t3, t3, t2); // t1 = u^2, t2 = v^2, t3 = 2*u*v
      rem(t1, t1, n);
      mul(t4, t1, t);
      add(t4, t4, t3);
      rem(u, t4, n);

      mul(t4, t1, ma);
      add(t4, t4, t2);
      rem(v, t4, n);

      if (bit(e, i)) {
         MulMod(t1, u, t, n);
         AddMod(t1, t1, v, n);
         MulMod(v, u, ma, n);
         u = t1;
      }

   }

   x = v;
}

// Chinese Remaindering.
//
// This version in new to v3.7, and is significantly
// simpler and faster than the previous version.
//
// This function takes as input g, a, G, p,
// such that a > 0, 0 <= G < p, and gcd(a, p) = 1.
// It computes a' = a*p and g' such that
//   * g' = g (mod a);
//   * g' = G (mod p);
//   * -a'/2 < g' <= a'/2.
// It then sets g := g' and a := a', and returns 1 iff g has changed.
//
// Under normal use, the input value g satisfies -a/2 < g <= a/2;
// however, this was not documented or enforced in earlier versions,
// so to maintain backward compatability, no restrictions are placed
// on g.  This routine runs faster, though, if -a/2 < g <= a/2,
// and the first thing the routine does is to make this condition
// hold.
//
// Also, under normal use, both a and p are odd;  however, the routine
// will still work even if this is not so.
//
// The routine is based on the following simple fact.
//
// Let -a/2 < g <= a/2, and let h satisfy
//   * g + a h = G (mod p);
//   * -p/2 < h <= p/2.
// Further, if p = 2*h and g > 0, set
//   g' := g - a h;
// otherwise, set
//   g' := g + a h.
// Then g' so defined satisfies the above requirements.
//
// It is trivial to see that g's satisfies the congruence conditions.
// The only thing is to check that the "balancing" condition
// -a'/2 < g' <= a'/2 also holds.

long CRT(ZZ& gg, ZZ& a, long G, long p)
{
   if (p >= NTL_SP_BOUND) {
      ZZ GG, pp;
      conv(GG, G);
      conv(pp, p);
      return CRT(gg, a, GG, pp);
   }

   long modified = 0;

   NTL_ZZRegister(g);

   if (!CRTInRange(gg, a)) {
      modified = 1;
      ZZ a1;
      rem(g, gg, a);
      RightShift(a1, a, 1);
      if (g > a1) sub(g, g, a);
   }
   else
      g = gg;

   long p1;
   p1 = p >> 1;

   long a_inv;
   a_inv = rem(a, p);
   a_inv = InvMod(a_inv, p);

   long h;
   h = rem(g, p);
   h = SubMod(G, h, p);
   h = MulMod(h, a_inv, p);
   if (h > p1)
      h = h - p;

   if (h != 0) {
      modified = 1;

      if (!(p & 1) && g > 0 && (h == p1))
         MulSubFrom(g, a, h);
      else
         MulAddTo(g, a, h);
   }

   mul(a, a, p);
   gg = g;

   return modified;
}

long CRT(ZZ& gg, ZZ& a, const ZZ& G, const ZZ& p)
{
   long modified = 0;

   ZZ g;

   if (!CRTInRange(gg, a)) {
      modified = 1;
      ZZ a1;
      rem(g, gg, a);
      RightShift(a1, a, 1);
      if (g > a1) sub(g, g, a);
   }
   else
      g = gg;

   ZZ p1;
   RightShift(p1, p, 1);

   ZZ a_inv;
   rem(a_inv, a, p);
   InvMod(a_inv, a_inv, p);

   ZZ h;
   rem(h, g, p);
   SubMod(h, G, h, p);
   MulMod(h, h, a_inv, p);
   if (h > p1)
      sub(h, h, p);

   if (h != 0) {
      modified = 1;
      ZZ ah;
      mul(ah, a, h);

      if (!IsOdd(p) && g > 0 &&  (h == p1))
         sub(g, g, ah);
      else
         add(g, g, ah);
   }

   mul(a, a, p);
   gg = g;

   return modified;
}

void sub(ZZ& x, const ZZ& a, long b)
{
   NTL_ZZRegister(B);
   conv(B, b);
   sub(x, a, B);
}

void sub(ZZ& x, long a, const ZZ& b)
{
   NTL_ZZRegister(A);
   conv(A, a);
   sub(x, A, b);
}

void power2(ZZ& x, long e)
{
   if (e < 0) ArithmeticError("power2: negative exponent");
   set(x);
   LeftShift(x, x, e);
}

void conv(ZZ& x, const char *s)
{
   long c;
   long cval;
   long sign;
   long ndigits;
   long acc;
   long i = 0;

   NTL_ZZRegister(a);

   if (!s) InputError("bad ZZ input");

   if (!iodigits) InitZZIO();

   a = 0;

   c = s[i];
   while (IsWhiteSpace(c)) {
      i++;
      c = s[i];
   }

   if (c == '-') {
      sign = -1;
      i++;
      c = s[i];
   }
   else
      sign = 1;

   cval = CharToIntVal(c);
   if (cval < 0 || cval > 9) InputError("bad ZZ input");

   ndigits = 0;
   acc = 0;
   while (cval >= 0 && cval <= 9) {
      acc = acc*10 + cval;
      ndigits++;

      if (ndigits == iodigits) {
         mul(a, a, ioradix);
         add(a, a, acc);
         ndigits = 0;
         acc = 0;
      }

      i++;
      c = s[i];
      cval = CharToIntVal(c);
   }

   if (ndigits != 0) {
      long mpy = 1;
      while (ndigits > 0) {
         mpy = mpy * 10;
         ndigits--;
      }

      mul(a, a, mpy);
      add(a, a, acc);
   }

   if (sign == -1)
      negate(a, a);

   x = a;
}

void bit_and(ZZ& x, const ZZ& a, long b)
{
   NTL_ZZRegister(B);
   conv(B, b);
   bit_and(x, a, B);
}

void bit_or(ZZ& x, const ZZ& a, long b)
{
   NTL_ZZRegister(B);
   conv(B, b);
   bit_or(x, a, B);
}

void bit_xor(ZZ& x, const ZZ& a, long b)
{
   NTL_ZZRegister(B);
   conv(B, b);
   bit_xor(x, a, B);
}

long power_long(long a, long e)
{
   if (e < 0) ArithmeticError("power_long: negative exponent");

   if (e == 0) return 1;

   if (a == 1) return 1;
   if (a == -1) {
      if (e & 1)
         return -1;
      else
         return 1;
   }

   // no overflow check --- result is computed correctly
   // modulo word size

   unsigned long res = 1;
   unsigned long aa = a;
   long i;

   for (i = 0; i < e; i++)
      res *= aa;

   return to_long(res);
}

//  RANDOM NUMBER GENERATION

// Idea for this PRNG.  Iteratively hash seed using md5
// to get 256 bytes to initialize arc4.
// Then use arc4 to get a pseudo-random byte stream.

// I've taken care that the pseudo-random numbers generated by
// the routines RandomBnd, RandomBits, and RandomLen
// are completely platform independent.

// I make use of the md5 compression function,
// which I've modified to work on 64-bit machines

/*
 *  BEGIN RSA's md5 stuff
 *
 */

/*
 **********************************************************************
 ** md5.c                                                            **
 ** RSA Data Security, Inc. MD5 Message Digest Algorithm             **
 ** Created: 2/17/90 RLR                                             **
 ** Revised: 1/91 SRD,AJ,BSK,JT Reference C Version                  **
 **********************************************************************
 */

/*
 **********************************************************************
 ** Copyright (C) 1990, RSA Data Security, Inc. All rights reserved. **
 **                                                                  **
 ** License to copy and use this software is granted provided that   **
 ** it is identified as the "RSA Data Security, Inc. MD5 Message     **
 ** Digest Algorithm" in all material mentioning or referencing this **
 ** software or this function.                                       **
 **                                                                  **
 ** License is also granted to make and use derivative works         **
 ** provided that such works are identified as "derived from the RSA **
 ** Data Security, Inc. MD5 Message Digest Algorithm" in all         **
 ** material mentioning or referencing the derived work.             **
 **                                                                  **
 ** RSA Data Security, Inc. makes no representations concerning      **
 ** either the merchantability of this software or the suitability   **
 ** of this software for any particular purpose.  It is provided "as **
 ** is" without express or implied warranty of any kind.             **
 **                                                                  **
 ** These notices must be retained in any copies of any part of this **
 ** documentation and/or software.                                   **
 **********************************************************************
 */

#if (NTL_BITS_PER_LONG <= 32)
#define TRUNC32(x) (x)
#else
#define TRUNC32(x) ((x) & ((1UL << 32)-1UL))
#endif

/* F, G and H are basic MD5 functions: selection, majority, parity */
#define F(x, y, z) (((x) & (y)) | ((~x) & (z)))
#define G(x, y, z) (((x) & (z)) | ((y) & (~z)))
#define H(x, y, z) ((x) ^ (y) ^ (z))
#define I(x, y, z) (TRUNC32((y) ^ ((x) | (~z))))

/* ROTATE_LEFT rotates x left n bits */
#define ROTATE_LEFT(x, n) (TRUNC32(((x) << (n)) | ((x) >> (32-(n)))))

/* FF, GG, HH, and II transformations for rounds 1, 2, 3, and 4 */
/* Rotation is separate from addition to prevent recomputation */
#define FF(a, b, c, d, x, s, ac) \
  {(a) = TRUNC32((a) + F((b), (c), (d)) + (x) + (ac)); \
   (a) = ROTATE_LEFT((a), (s)); \
   (a) = TRUNC32((a) + (b)); \
  }
#define GG(a, b, c, d, x, s, ac) \
  {(a) = TRUNC32((a) + G((b), (c), (d)) + (x) + (ac)); \
   (a) = ROTATE_LEFT((a), (s)); \
   (a) = TRUNC32((a) + (b)); \
  }
#define HH(a, b, c, d, x, s, ac) \
  {(a) = TRUNC32((a) + H((b), (c), (d)) + (x) + (ac)); \
   (a) = ROTATE_LEFT((a), (s)); \
   (a) = TRUNC32((a) + (b)); \
  }
#define II(a, b, c, d, x, s, ac) \
  {(a) = TRUNC32((a) + I((b), (c), (d)) + (x) + (ac)); \
   (a) = ROTATE_LEFT((a), (s)); \
   (a) = TRUNC32((a) + (b)); \
  }

static
void MD5_default_IV(unsigned long *buf)
{
   buf[0] = 0x67452301UL;
   buf[1] = 0xefcdab89UL;
   buf[2] = 0x98badcfeUL;
   buf[3] = 0x10325476UL;
}

/* Basic MD5 step. Transform buf based on in.
 */

static
void MD5_compress(unsigned long *buf, unsigned long *in)
{
  unsigned long a = buf[0], b = buf[1], c = buf[2], d = buf[3];

  /* Round 1 */
#define S11 7
#define S12 12
#define S13 17
#define S14 22
  FF ( a, b, c, d, in[ 0], S11, 3614090360UL); /* 1 */
  FF ( d, a, b, c, in[ 1], S12, 3905402710UL); /* 2 */
  FF ( c, d, a, b, in[ 2], S13,  606105819UL); /* 3 */
  FF ( b, c, d, a, in[ 3], S14, 3250441966UL); /* 4 */
  FF ( a, b, c, d, in[ 4], S11, 4118548399UL); /* 5 */
  FF ( d, a, b, c, in[ 5], S12, 1200080426UL); /* 6 */
  FF ( c, d, a, b, in[ 6], S13, 2821735955UL); /* 7 */
  FF ( b, c, d, a, in[ 7], S14, 4249261313UL); /* 8 */
  FF ( a, b, c, d, in[ 8], S11, 1770035416UL); /* 9 */
  FF ( d, a, b, c, in[ 9], S12, 2336552879UL); /* 10 */
  FF ( c, d, a, b, in[10], S13, 4294925233UL); /* 11 */
  FF ( b, c, d, a, in[11], S14, 2304563134UL); /* 12 */
  FF ( a, b, c, d, in[12], S11, 1804603682UL); /* 13 */
  FF ( d, a, b, c, in[13], S12, 4254626195UL); /* 14 */
  FF ( c, d, a, b, in[14], S13, 2792965006UL); /* 15 */
  FF ( b, c, d, a, in[15], S14, 1236535329UL); /* 16 */

  /* Round 2 */
#define S21 5
#define S22 9
#define S23 14
#define S24 20
  GG ( a, b, c, d, in[ 1], S21, 4129170786UL); /* 17 */
  GG ( d, a, b, c, in[ 6], S22, 3225465664UL); /* 18 */
  GG ( c, d, a, b, in[11], S23,  643717713UL); /* 19 */
  GG ( b, c, d, a, in[ 0], S24, 3921069994UL); /* 20 */
  GG ( a, b, c, d, in[ 5], S21, 3593408605UL); /* 21 */
  GG ( d, a, b, c, in[10], S22,   38016083UL); /* 22 */
  GG ( c, d, a, b, in[15], S23, 3634488961UL); /* 23 */
  GG ( b, c, d, a, in[ 4], S24, 3889429448UL); /* 24 */
  GG ( a, b, c, d, in[ 9], S21,  568446438UL); /* 25 */
  GG ( d, a, b, c, in[14], S22, 3275163606UL); /* 26 */
  GG ( c, d, a, b, in[ 3], S23, 4107603335UL); /* 27 */
  GG ( b, c, d, a, in[ 8], S24, 1163531501UL); /* 28 */
  GG ( a, b, c, d, in[13], S21, 2850285829UL); /* 29 */
  GG ( d, a, b, c, in[ 2], S22, 4243563512UL); /* 30 */
  GG ( c, d, a, b, in[ 7], S23, 1735328473UL); /* 31 */
  GG ( b, c, d, a, in[12], S24, 2368359562UL); /* 32 */

  /* Round 3 */
#define S31 4
#define S32 11
#define S33 16
#define S34 23
  HH ( a, b, c, d, in[ 5], S31, 4294588738UL); /* 33 */
  HH ( d, a, b, c, in[ 8], S32, 2272392833UL); /* 34 */
  HH ( c, d, a, b, in[11], S33, 1839030562UL); /* 35 */
  HH ( b, c, d, a, in[14], S34, 4259657740UL); /* 36 */
  HH ( a, b, c, d, in[ 1], S31, 2763975236UL); /* 37 */
  HH ( d, a, b, c, in[ 4], S32, 1272893353UL); /* 38 */
  HH ( c, d, a, b, in[ 7], S33, 4139469664UL); /* 39 */
  HH ( b, c, d, a, in[10], S34, 3200236656UL); /* 40 */
  HH ( a, b, c, d, in[13], S31,  681279174UL); /* 41 */
  HH ( d, a, b, c, in[ 0], S32, 3936430074UL); /* 42 */
  HH ( c, d, a, b, in[ 3], S33, 3572445317UL); /* 43 */
  HH ( b, c, d, a, in[ 6], S34,   76029189UL); /* 44 */
  HH ( a, b, c, d, in[ 9], S31, 3654602809UL); /* 45 */
  HH ( d, a, b, c, in[12], S32, 3873151461UL); /* 46 */
  HH ( c, d, a, b, in[15], S33,  530742520UL); /* 47 */
  HH ( b, c, d, a, in[ 2], S34, 3299628645UL); /* 48 */

  /* Round 4 */
#define S41 6
#define S42 10
#define S43 15
#define S44 21
  II ( a, b, c, d, in[ 0], S41, 4096336452UL); /* 49 */
  II ( d, a, b, c, in[ 7], S42, 1126891415UL); /* 50 */
  II ( c, d, a, b, in[14], S43, 2878612391UL); /* 51 */
  II ( b, c, d, a, in[ 5], S44, 4237533241UL); /* 52 */
  II ( a, b, c, d, in[12], S41, 1700485571UL); /* 53 */
  II ( d, a, b, c, in[ 3], S42, 2399980690UL); /* 54 */
  II ( c, d, a, b, in[10], S43, 4293915773UL); /* 55 */
  II ( b, c, d, a, in[ 1], S44, 2240044497UL); /* 56 */
  II ( a, b, c, d, in[ 8], S41, 1873313359UL); /* 57 */
  II ( d, a, b, c, in[15], S42, 4264355552UL); /* 58 */
  II ( c, d, a, b, in[ 6], S43, 2734768916UL); /* 59 */
  II ( b, c, d, a, in[13], S44, 1309151649UL); /* 60 */
  II ( a, b, c, d, in[ 4], S41, 4149444226UL); /* 61 */
  II ( d, a, b, c, in[11], S42, 3174756917UL); /* 62 */
  II ( c, d, a, b, in[ 2], S43,  718787259UL); /* 63 */
  II ( b, c, d, a, in[ 9], S44, 3951481745UL); /* 64 */

  buf[0] = TRUNC32(buf[0] + a);
  buf[1] = TRUNC32(buf[1] + b);
  buf[2] = TRUNC32(buf[2] + c);
  buf[3] = TRUNC32(buf[3] + d);
}

/*
 *  END RSA's md5 stuff
 *
 */

static
void words_from_bytes(unsigned long *txtl, const unsigned char *txtc, long n)
{
   long i;
   unsigned long v;

   for (i = 0; i < n; i++) {
      v = txtc[4*i];
      v += ((unsigned long) (txtc[4*i+1])) << 8;
      v += ((unsigned long) (txtc[4*i+2])) << 16;
      v += ((unsigned long) (txtc[4*i+3])) << 24;
      txtl[i] = v;
   }
}

static
void bytes_from_words(unsigned char *txtc, const unsigned long *txtl, long n)
{
   long i;
   unsigned long v;

   for (i = 0; i < n; i++) {
      v = txtl[i];
      txtc[4*i] = v & 255;
      v = v >> 8;
      txtc[4*i+1] = v & 255;
      v = v >> 8;
      txtc[4*i+2] = v & 255;
      v = v >> 8;
      txtc[4*i+3] = v & 255;
   }
}

static
void MD5_compress1(unsigned long *buf, unsigned char *in, long n)
{
   unsigned long txtl[16];
   unsigned char txtc[64];
   long i, j, k;

   if (n < 0) n = 0;

   i = 0;
   while (i < n) {
      k = n-i;
      if (k > 64) k = 64;
      for (j = 0; j < k; j++)
         txtc[j] = in[i+j];
      for (; j < 64; j++)
         txtc[j] = 0;
      words_from_bytes(txtl, txtc, 16);
      MD5_compress(buf, txtl);
      i += k;
   }
}

// the "cipherpunk" version of arc4

struct _ZZ_arc4_key
{
    unsigned char state[256];
    unsigned char x;
    unsigned char y;
};

static inline
void swap_byte(unsigned char *a, unsigned char *b)
{
    unsigned char swapByte;

    swapByte = *a;
    *a = *b;
    *b = swapByte;
}

static
void prepare_key(unsigned char *key_data_ptr,
                 long key_data_len, _ZZ_arc4_key *key)
{
    unsigned char index1;
    unsigned char index2;
    unsigned char* state;
    long counter;

    state = &key->state[0];
    for(counter = 0; counter < 256; counter++)
       state[counter] = counter;
    key->x = 0;
    key->y = 0;
    index1 = 0;
    index2 = 0;
    for(counter = 0; counter < 256; counter++)
    {
         index2 = (key_data_ptr[index1] + state[counter] + index2) & 255;
         swap_byte(&state[counter], &state[index2]);

         index1 = (index1 + 1) % key_data_len;
    }
}

static
void arc4(unsigned char *buffer_ptr, long buffer_len, _ZZ_arc4_key *key)
{
    unsigned char x;
    unsigned char y;
    unsigned char* state;
    unsigned char xorIndex;
    long counter;

    x = key->x;
    y = key->y;

    state = &key->state[0];
    for(counter = 0; counter < buffer_len; counter ++)
    {
         x = (x + 1) & 255;
         y = (state[x] + y) & 255;
         swap_byte(&state[x], &state[y]);

         xorIndex = (state[x] + state[y]) & 255;

         buffer_ptr[counter] = state[xorIndex];
     }
     key->x = x;
     key->y = y;
}

// global state information for PRNG

NTL_THREAD_LOCAL static long ran_initialized = 0;
NTL_THREAD_LOCAL static _ZZ_arc4_key ran_key;

static const unsigned long default_md5_tab[16] = {
744663023UL, 1011602954UL, 3163087192UL, 3383838527UL,
3305324122UL, 3197458079UL, 2266495600UL, 2760303563UL,
346234297UL, 1919920720UL, 1896169861UL, 2192176675UL,
2027150322UL, 2090160759UL, 2134858730UL, 1131796244UL
};

static
void build_arc4_tab(unsigned char *seed_bytes, const ZZ& s)
{
   long nb = NumBytes(s);

   unsigned char *txt;

   Vec<unsigned char> txt_storage;
   txt_storage.SetLength(nb + 68);
   txt = txt_storage.elts();

   BytesFromZZ(txt + 4, s, nb);

   bytes_from_words(txt + nb + 4, default_md5_tab, 16);

   unsigned long buf[4];

   unsigned long i;
   for (i = 0; i < 16; i++) {
      MD5_default_IV(buf);
      bytes_from_words(txt, &i, 1);

      MD5_compress1(buf, txt, nb + 68);

      bytes_from_words(seed_bytes + 16*i, buf, 4);
   }
}

void SetSeed(const ZZ& s)
{
   unsigned char seed_bytes[256];

   build_arc4_tab(seed_bytes, s);
   prepare_key(seed_bytes, 256, &ran_key);

   // we discard the first 1024 bytes of the arc4 stream, as this is
   // recommended practice.

   arc4(seed_bytes, 256, &ran_key);
   arc4(seed_bytes, 256, &ran_key);
   arc4(seed_bytes, 256, &ran_key);
   arc4(seed_bytes, 256, &ran_key);

   ran_initialized = 1;
}

static
void ran_bytes(unsigned char *bytes, long n)
{
   if (!ran_initialized) {
      ZZ x;
      const string& id = UniqueID();
      ZZFromBytes(x, (const unsigned char *) id.c_str(), id.length());
      SetSeed(x);
   }
   arc4(bytes, n, &ran_key);
}

unsigned long RandomWord()
{
   unsigned char buf[NTL_BITS_PER_LONG/8];
   long i;
   unsigned long res;

   ran_bytes(buf, NTL_BITS_PER_LONG/8);

   res = 0;
   for (i = NTL_BITS_PER_LONG/8 - 1; i >= 0; i--) {
      res = res << 8;
      res = res | buf[i];
   }

   return res;
}

long RandomBits_long(long l)
{
   if (l <= 0) return 0;
   if (l >= NTL_BITS_PER_LONG)
      ResourceError("RandomBits: length too big");

   unsigned char buf[NTL_BITS_PER_LONG/8];
   unsigned long res;
   long i;

   long nb = (l+7)/8;
   ran_bytes(buf, nb);

   res = 0;
   for (i = nb - 1; i >= 0; i--) {
      res = res << 8;
      res = res | buf[i];
   }

   return long(res & ((1UL << l)-1UL));
}

unsigned long RandomBits_ulong(long l)
{
   if (l <= 0) return 0;
   if (l > NTL_BITS_PER_LONG)
      ResourceError("RandomBits: length too big");

   unsigned char buf[NTL_BITS_PER_LONG/8];
   unsigned long res;
   long i;

   long nb = (l+7)/8;
   ran_bytes(buf, nb);

   res = 0;
   for (i = nb - 1; i >= 0; i--) {
      res = res << 8;
      res = res | buf[i];
   }

   if (l < NTL_BITS_PER_LONG)
      res = res & ((1UL << l)-1UL);

   return res;
}

long RandomLen_long(long l)
{
   if (l <= 0) return 0;
   if (l == 1) return 1;
   if (l >= NTL_BITS_PER_LONG)
      ResourceError("RandomLen: length too big");

   return RandomBits_long(l-1) + (1L << (l-1));
}

void RandomBits(ZZ& x, long l)
{
   if (l <= 0) {
      x = 0;
      return;
   }

   if (NTL_OVERFLOW(l, 1, 0))
      ResourceError("RandomBits: length too big");

   long nb = (l+7)/8;

   NTL_THREAD_LOCAL static Vec<unsigned char> buf_mem;
   Vec<unsigned char>::Watcher watch_buf_mem(buf_mem);

   buf_mem.SetLength(nb);
   unsigned char *buf = buf_mem.elts();

   ran_bytes(buf, nb);

   NTL_ZZRegister(res);

   ZZFromBytes(res, buf, nb);
   trunc(res, res, l);

   x = res;
}

void RandomLen(ZZ& x, long l)
{
   if (l <= 0) {
      x = 0;
      return;
   }

   if (l == 1) {
      x = 1;
      return;
   }

   if (NTL_OVERFLOW(l, 1, 0))
      ResourceError("RandomLen: length too big");

   // pre-allocate space to avoid two allocations
   long nw = (l + NTL_ZZ_NBITS - 1)/NTL_ZZ_NBITS;
   x.SetSize(nw);

   RandomBits(x, l-1);
   SetBit(x, l-1);
}

const long RandomBndExcess = 8;

void RandomBnd(ZZ& x, const ZZ& bnd)
{
   if (bnd <= 1) {
      x = 0;
      return;
   }

   long k = NumBits(bnd);

   if (weight(bnd) == 1) {
      RandomBits(x, k-1);
      return;
   }

   long l = k + RandomBndExcess;

   NTL_ZZRegister(t);
   NTL_ZZRegister(r);
   NTL_ZZRegister(t1);

   do {
      RandomBits(t, l);
      rem(r, t, bnd);
      sub(t1, bnd, r);
      add(t, t, t1);
   } while (NumBits(t) > l);

   x = r;
}

long RandomBnd(long bnd)
{
   if (bnd <= 1) return 0;

   long k = NumBits(bnd);

   if (((bnd - 1) & bnd) == 0)
      return RandomBits_long(k-1);

   long l = k + RandomBndExcess;

   if (l > NTL_BITS_PER_LONG-2) {
      NTL_ZZRegister(Bnd);
      NTL_ZZRegister(res);

      Bnd = bnd;
      RandomBnd(res, Bnd);
      return to_long(res);
   }

   long t, r;

   do {
      t = RandomBits_long(l);
      r = t % bnd;
   } while (t + bnd - r > (1L << l));

   return r;
}

// More prime generation stuff...

static
double Log2(double x)
{
   NTL_THREAD_LOCAL static double log2 = log(2.0);
   return log(x)/log2;
}

// Define p(k,t) to be the conditional probability that a random, odd, k-bit
// number is composite, given that it passes t iterations of the
// Miller-Rabin test.
// This routine returns 0 or 1, and if it returns 1 then
// p(k,t) <= 2^{-n}.
// This basically encodes the estimates of Damgard, Landrock, and Pomerance;
// it uses floating point arithmetic, but is coded in such a way
// that its results should be correct, assuming that the log function
// is computed with reasonable precision.
//
// It is assumed that k >= 3 and t >= 1; if this does not hold,
// then 0 is returned.

static
long ErrBoundTest(long kk, long tt, long nn)

{
   const double fudge = (1.0 + 1024.0/NTL_FDOUBLE_PRECISION);
   const double log2_3 = Log2(3.0);
   const double log2_7 = Log2(7.0);
   const double log2_20 = Log2(20.0);

   double k = kk;
   double t = tt;
   double n = nn;

   if (k < 3 || t < 1) return 0;
   if (n < 1) return 1;

   // the following test is largely academic
   if (9*t > NTL_FDOUBLE_PRECISION) LogicError("ErrBoundTest: t too big");

   double log2_k = Log2(k);

   if ((n + log2_k)*fudge <= 2*t)
      return 1;

   if ((2*log2_k + 4.0 + n)*fudge <= 2*sqrt(k))
      return 2;

   if ((t == 2 && k >= 88) || (3 <= t && 9*t <= k && k >= 21)) {
      if ((1.5*log2_k + t + 4.0 + n)*fudge <= 0.5*Log2(t) + 2*(sqrt(t*k)))
         return 3;
   }

   if (k <= 9*t && 4*t <= k && k >= 21) {
      if ( ((log2_3 + log2_7 + log2_k + n)*fudge <= log2_20 + 5*t)  &&
           ((log2_3 + (15.0/4.0)*log2_k + n)*fudge <= log2_7 + k/2 + 2*t) &&
           ((2*log2_3 + 2 + log2_k + n)*fudge <= k/4 + 3*t) )
         return 4;
   }

   if (4*t >= k && k >= 21) {
      if (((15.0/4.0)*log2_k + n)*fudge <= log2_7 + k/2 + 2*t)
         return 5;
   }

   return 0;
}

void GenPrime(ZZ& n, long k, long err)
{
   if (k <= 1) LogicError("GenPrime: bad length");

   if (k > (1L << 20)) ResourceError("GenPrime: length too large");

   if (err < 1) err = 1;
   if (err > 512) err = 512;

   if (k == 2) {
      if (RandomBnd(2))
         n = 3;
      else
         n = 2;

      return;
   }

   long t;

   t = 1;
   while (!ErrBoundTest(k, t, err))
      t++;

   RandomPrime(n, k, t);
}

long GenPrime_long(long k, long err)
{
   if (k <= 1) LogicError("GenPrime: bad length");

   if (k >= NTL_BITS_PER_LONG) ResourceError("GenPrime: length too large");

   if (err < 1) err = 1;
   if (err > 512) err = 512;

   if (k == 2) {
      if (RandomBnd(2))
         return 3;
      else
         return 2;
   }

   long t;

   t = 1;
   while (!ErrBoundTest(k, t, err))
      t++;

   return RandomPrime_long(k, t);
}

void GenGermainPrime(ZZ& n, long k, long err)
{
   if (k <= 1) LogicError("GenGermainPrime: bad length");

   if (k > (1L << 20)) ResourceError("GenGermainPrime: length too large");

   if (err < 1) err = 1;
   if (err > 512) err = 512;

   if (k == 2) {
      if (RandomBnd(2))
         n = 3;
      else
         n = 2;

      return;
   }

   long prime_bnd = ComputePrimeBound(k);

   if (NumBits(prime_bnd) >= k/2)
      prime_bnd = (1L << (k/2-1));

   ZZ two;
   two = 2;

   ZZ n1;

   PrimeSeq s;

   ZZ iter;
   iter = 0;

   for (;;) {
      iter++;

      RandomLen(n, k);
      if (!IsOdd(n)) add(n, n, 1);

      s.reset(3);
      long p;

      long sieve_passed = 1;

      p = s.next();
      while (p && p < prime_bnd) {
         long r = rem(n, p);

         if (r == 0) {
            sieve_passed = 0;
            break;
         }

         // test if 2*r + 1 = 0 (mod p)
         if (r == p-r-1) {
            sieve_passed = 0;
            break;
         }

         p = s.next();
      }

      if (!sieve_passed) continue;

      if (MillerWitness(n, two)) continue;

      // n1 = 2*n+1
      mul(n1, n, 2);
      add(n1, n1, 1);

      if (MillerWitness(n1, two)) continue;

      // now do t M-R iterations...just to make sure

      // First compute the appropriate number of M-R iterations, t
      // The following computes t such that
      //       p(k,t)*8/k <= 2^{-err}/(5*iter^{1.25})
      // which suffices to get an overall error probability of 2^{-err}.
      // Note that this method has the advantage of not requiring
      // any assumptions on the density of Germain primes.

      long err1 = max(1, err + 7 + (5*NumBits(iter) + 3)/4 - NumBits(k));
      long t;
      t = 1;
      while (!ErrBoundTest(k, t, err1))
         t++;

      ZZ W;
      long MR_passed = 1;

      long i;
      for (i = 1; i <= t; i++) {
         do {
            RandomBnd(W, n);
         } while (W == 0);
         // W == 0 is not a useful candidate witness!

         if (MillerWitness(n, W)) {
            MR_passed = 0;
            break;
         }
      }

      if (MR_passed) break;
   }
}

long GenGermainPrime_long(long k, long err)
{
   if (k >= NTL_BITS_PER_LONG-1)
      ResourceError("GenGermainPrime_long: length too long");

   ZZ n;
   GenGermainPrime(n, k, err);
   return to_long(n);
}

NTL_END_IMPL


/*
  * qGeod
  *
*/

#define _MAIN_

#include <iostream>
#include <armadillo>

#include "../core/UAmoeba.hpp"

#include "../io/cxmatLoad.cpp"

#ifdef _ALGEBRA_TOOLS_
#else
#include "../core/algebraTools.hpp"
#endif

#ifdef _AMOEBA_INIT_
#else
#include "../core/amoebaParam.cpp"
#endif

#include "../core/translate.cpp"

#include "../solvers/segSolver.cpp"
#include "../solvers/serialSolver.cpp"
#include "../solvers/monte.cpp"

using namespace arma;

int main(int argc, char **argv)
{
	int rank;
	int size;

	MPI_Init(&argc, &argv);
	MPI_Comm_size(MPI_COMM_WORLD, &size);
	MPI_Comm_rank(MPI_COMM_WORLD, &rank);

	cx_mat startBoundary;
	cx_mat endBoundary;

	int maxIters;
	int precision;
	int matSize;
	int parallelFlag;

	if(argc==1)
	{
		cout<< "Usage : /qGeod <0/1/2> start.mat target.mat n maxAmoebaIters nGridPoints precision maxMainIters penalty\n"
				<< "=================================================================================================== \n"
				<< "n   				: exponent in SU(2^n)                                  \n"
				<< "precision   : working accuracy for qGeod                           \n"
				<< "maxIters    : maximum number of iterations in leap-frog            \n"
				<< "target.mat  : matrix containing desired unitary operation,         \n"
				<< "              in format described by                               \n"
			  << "              http://arma.sourceforge.net/docs.html#save_load_mat  \n"
				<< "nGridPoints : number of guess points in amoeba                     \n"
				<< "0/1/2       : run in serial 0, parallel 1, 2 monte carlo           \n"
				<< "=================================================================================================== \n";
	}
	else
	{

		parallelFlag = atoi(argv[1]);
		char *sBoundary = argv[2];
		char *eBoundary = argv[3];
		matSize = pow(2,atoi(argv[4]));

		cout << "Loading target matrix \n";
		cxmatLoad(endBoundary, matSize, eBoundary);
		cxmatLoad(startBoundary, matSize, sBoundary);

		cout << "Initialising solver parameters\n";
		AmoebaParam<cx_mat> amoebaParam;
		amoebaParam.getData(argc, argv);

		amoebaParam.startBoundary = startBoundary;
		amoebaParam.endBoundary = endBoundary;

		switch(parallelFlag)
		{
			case 0:
				cout << "Running serial solver\n";
				serialSolver<cx_mat>(amoebaParam);
				break;
			case 1:
				cout << "Running leap-frog solver\n";
				segSolver<cx_mat>(amoebaParam, rank, size);
				break;
			case 2:
				cout << "Running Monte Carlo solver\n";
				monteCarlo<cx_mat>(amoebaParam);
		}
	}

	MPI_Finalize();
	return 0;
}


#pragma once

#include <map>

#include <Eigen/Sparse>

#include "eigen.hh"
#include "util/optional.hh"

namespace estimation {
namespace optimization {

// BSR Matrix
class BlockSparseMatrix {
  using SpMat = Eigen::SparseMatrix<double>;
  using SpVec = Eigen::SparseVector<double>;

 public:
  BlockSparseMatrix() {
    valid_ = false;
  }
  BlockSparseMatrix(const int n_block_rows, const int n_block_cols);

  void set(int row, int col, const Eigen::MatrixXd& mat);
  const Eigen::MatrixXd& get(int row, int col) const;

  int real_rows() const;
  int real_cols() const;

  int real_rows_above_block(int block_row) const;
  int real_cols_left_of_block(int block_col) const;

  SpMat to_eigen_sparse() const;

  jcc::Optional<VecXd> solve_lst_sq(const std::vector<VecXd>& residuals,
                                    const BlockSparseMatrix& R_inv,
                                    const double lambda = 0.0) const;

  int block_rows() const {
    return rows_.size();
  }
  int block_cols() const {
    return n_cols_.size();
  }

  // Here's a little mock for what an LDLT will feel like.
  /*
  void ldlt() {
    D;
    L;

    for (int j = 0; j < i; ++j) {
      DDii = 0;
      for (int k = 0; k < j; ++k) {
        DDii += L[j, k] * D[k] * L[j, k].transpose();
      }
      D[j] = A[j, j] - DDii;

      for (int i = 0; i < n; ++i) {
        DD = 0;

        d_llt = llt(D[j]);
        for (int k = 0; k < j; ++k) {
          DD += L(i, k) * D[k] * L[j, k].transpose();
        }
        L[i, j] = d_llt.solve((A[i, j] - DD).transpose()).transpose();
      }
    }
  }
  */

 private:
  struct Block {
    Eigen::MatrixXd block;
  };

  struct Row {
    std::map<int, Block> cols;
    int n_rows = -1;
  };

  // The number of columns in each column block
  std::vector<int> n_cols_;
  // The block rows
  std::vector<Row> rows_;

  bool valid_ = false;
};
}  // namespace optimization
}  // namespace estimation


//  ================================================================
//  Created by Gregory Kramida on 10/23/18.
//  Copyright (c) 2018 Gregory Kramida
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at

//  http://www.apache.org/licenses/LICENSE-2.0

//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//  ================================================================
#pragma once

//libraries
#include <Eigen/Eigen>

//local
#include "../../math/typedefs.hpp"

namespace eig = Eigen;

namespace nonrigid_optimization {
namespace slavcheva{

void
compute_tikhonov_regularization_gradient(math::MatrixXv2f& gradient, float& energy, const math::MatrixXv2f& warp_field);

void
compute_tikhonov_regularization_gradient_within_band_union(math::MatrixXv2f& gradient, float& energy,
                                                           const math::MatrixXv2f& warp_field,
                                                           const eig::MatrixXf& live_field,
                                                           const eig::MatrixXf& canonical_field);
} //namespace slavcheva
}//nonrigid_optimization


#ifndef COMMON_HPP_INCLUDED
#define COMMON_HPP_INCLUDED

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>

static constexpr std::size_t n = 8;
static constexpr std::size_t N = n*n;

using Matrix = boost::numeric::ublas::compressed_matrix<double>;
using Vector = boost::numeric::ublas::vector<double>;
using Index = std::remove_const_t<decltype(N)>;
using Color = Index;
using Block = Index;
using Level = Index;

#endif


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 24, 2017 $
//   Revision:            $Revision: 1.0 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "python/pointer_vector_set_python_interface.h"
#include "custom_utilities/patch.h"
#include "custom_utilities/control_grid_utility.h"
#include "custom_utilities/hbsplines/deprecated_hb_mesh.h"
#include "custom_utilities/hbsplines/hbsplines_basis_function.h"
#include "custom_utilities/hbsplines/hbsplines_fespace.h"
#include "custom_utilities/hbsplines/hbsplines_patch_utility.h"
#include "custom_utilities/hbsplines/hbsplines_refinement_utility.h"
#include "custom_utilities/import_export/multi_hbsplines_patch_matlab_exporter.h"
#include "custom_python/iga_define_python.h"
#include "custom_python/add_hbsplines_to_python.h"
#include "custom_python/add_point_based_control_grid_to_python.h"
#include "custom_python/add_import_export_to_python.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

////////////////////////////////////////

// template<int TDim>
// std::size_t HBSplinesBasisFunction_GetId(HBSplinesBasisFunction<TDim>& rDummy)
// {
//     return rDummy.Id();
// }

template<int TDim>
void HBSplinesBasisFunction_SetId(HBSplinesBasisFunction<TDim>& rDummy, std::size_t Id)
{
    // DO NOTHING
}

template<int TDim>
std::size_t HBSplinesBasisFunction_GetLevel(HBSplinesBasisFunction<TDim>& rDummy)
{
    return rDummy.Level();
}

// template<int TDim>
// std::size_t HBSplinesBasisFunction_GetEquationId(HBSplinesBasisFunction<TDim>& rDummy)
// {
//     return rDummy.EquationId();
// }

// template<int TDim>
// void HBSplinesBasisFunction_SetEquationId(HBSplinesBasisFunction<TDim>& rDummy, std::size_t EquationId)
// {
//     rDummy.SetEquationId(EquationId);
// }

template<int TDim>
boost::python::list HBSplinesFESpace_ExtractBoundaryBfsByFlag(HBSplinesFESpace<TDim>& rDummy, std::size_t boundary_id)
{
    typedef typename HBSplinesFESpace<TDim>::bf_t bf_t;

    std::vector<bf_t> bf_list = rDummy.ExtractBoundaryBfsByFlag(boundary_id);

    boost::python::list Output;
    for (std::size_t i = 0; i < bf_list.size(); ++i)
        Output.append(bf_list[i]);

    return Output;
}

template<int TDim>
std::size_t HBSplinesFESpace_MaxLevel(HBSplinesFESpace<TDim>& rDummy)
{
    return rDummy.MaxLevel();
}

template<int TDim>
typename HBSplinesBasisFunction<TDim>::Pointer HBSplinesFESpace_GetItem(HBSplinesFESpace<TDim>& rDummy, std::size_t i)
{
    return rDummy[i];
}

////////////////////////////////////////

template<int TDim>
void HBSplinesPatchUtility_ListBoundaryBfs(HBSplinesPatchUtility& rDummy,
    typename HBSplinesFESpace<TDim>::Pointer pFESpace, BoundarySide side)
{
    rDummy.ListBoundaryBfs<TDim>(std::cout, pFESpace, side);
}

template<int TDim>
typename Patch<TDim>::Pointer HBSplinesPatchUtility_CreatePatchFromBSplines(HBSplinesPatchUtility& rDummy,
    typename Patch<TDim>::Pointer pPatch)
{
    return HBSplinesPatchUtility::CreatePatchFromBSplines<TDim>(pPatch);
}

template<int TDim>
typename HBSplinesFESpace<TDim>::bf_t HBSplinesPatchUtility_GetBfByEquationId(HBSplinesPatchUtility& rDummy,
    typename MultiPatch<TDim>::Pointer pMultiPatch, const std::size_t& EquationId)
{
    return rDummy.GetBfByEquationId<TDim>(pMultiPatch, EquationId);
}

template<int TDim>
void HBSplinesPatchUtility_ReportDuplicatedEquationId(HBSplinesPatchUtility& rDummy,
    typename MultiPatch<TDim>::Pointer pMultiPatch, const bool& throw_error)
{
    return rDummy.ReportDuplicatedEquationId<TDim>(pMultiPatch, throw_error);
}

////////////////////////////////////////

template<int TDim>
void HBSplinesRefinementUtility_Refine(HBSplinesRefinementUtility& rDummy,
        typename Patch<TDim>::Pointer pPatch, const std::size_t& Id, const int& EchoLevel)
{
    rDummy.Refine<TDim>(pPatch, Id, EchoLevel);
}

template<int TDim>
void HBSplinesRefinementUtility_RefineBf(HBSplinesRefinementUtility& rDummy,
        typename Patch<TDim>::Pointer pPatch, typename HBSplinesFESpace<TDim>::bf_t p_bf, const int& EchoLevel)
{
    rDummy.Refine<TDim>(pPatch, p_bf, EchoLevel);
}

template<int TDim>
void HBSplinesRefinementUtility_RefineWindow(HBSplinesRefinementUtility& rDummy,
        typename Patch<TDim>::Pointer pPatch, boost::python::list& window, const int& EchoLevel)
{
    std::vector<std::vector<double> > window_vector;
    std::size_t cnt1 = 0, cnt2 = 0;
    typedef boost::python::stl_input_iterator<boost::python::list> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& vect, std::make_pair(iterator_value_type(window), iterator_value_type() ) )
    {
        typedef boost::python::stl_input_iterator<double> iterator_value_type2;
        std::vector<double> win_vect;
        BOOST_FOREACH(const iterator_value_type2::value_type& v, std::make_pair(iterator_value_type2(vect), iterator_value_type2() ) )
        {
            win_vect.push_back(v);
        }
        window_vector.push_back(win_vect);
    }
    rDummy.RefineWindow<TDim>(pPatch, window_vector, EchoLevel);
}

template<int TDim>
void HBSplinesRefinementUtility_LinearDependencyRefine(HBSplinesRefinementUtility& rDummy,
        typename Patch<TDim>::Pointer pPatch, const std::size_t& refine_cycle, const int& EchoLevel)
{
    rDummy.LinearDependencyRefine<TDim>(pPatch, refine_cycle, EchoLevel);
}

////////////////////////////////////////

// template<typename TDataType, class TFESpaceType>
// typename ControlGrid<TDataType>::Pointer ControlGridUtility_CreatePointBasedControlGrid(
//         ControlGridUtility& rDummy,
//         const Variable<TDataType>& rVariable, typename TFESpaceType::Pointer pFESpace)
// {
//     return rDummy.CreatePointBasedControlGrid<TDataType, TFESpaceType>(rVariable, pFESpace);
// }

////////////////////////////////////////

template<int TDim>
void IsogeometricApplication_AddHBSplinesSpaceToPython()
{

    std::stringstream ss;

    ss.str(std::string());
    ss << "HBSplinesBasisFunction" << TDim << "D";
    class_<HBSplinesBasisFunction<TDim>, typename HBSplinesBasisFunction<TDim>::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<const std::size_t&, const std::size_t&>())
    // .add_property("Id", HBSplinesBasisFunction_GetId<TDim>, HBSplinesBasisFunction_SetId<TDim>)
    .add_property("Id", Isogeometric_GetId<HBSplinesBasisFunction<TDim> >, HBSplinesBasisFunction_SetId<TDim>)
    // .add_property("EquationId", HBSplinesBasisFunction_GetEquationId<TDim>, HBSplinesBasisFunction_SetEquationId<TDim>)
    .add_property("EquationId", Isogeometric_GetEquationId<HBSplinesBasisFunction<TDim>>, Isogeometric_SetEquationId<HBSplinesBasisFunction<TDim>>)
    .def("Weight", &HBSplinesBasisFunction<TDim>::Weight)
    .def("Level", &HBSplinesBasisFunction_GetLevel<TDim>)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "HBSplinesFESpace" << TDim << "D";
//    typename FESpace<TDim-1>::Pointer(HBSplinesFESpace<TDim>::*pointer_to_ConstructBoundaryFESpace1)(const BoundarySide& side) const = &HBSplinesFESpace<TDim>::ConstructBoundaryFESpace;
    // typename FESpace<TDim-1>::Pointer(HBSplinesFESpace<TDim>::*pointer_to_ConstructBoundaryFESpace2)(const BoundarySide& side, const BoundaryRotation& rotation) const = &HBSplinesFESpace<TDim>::ConstructBoundaryFESpace;
    class_<HBSplinesFESpace<TDim>, typename HBSplinesFESpace<TDim>::Pointer, bases<FESpace<TDim> >, boost::noncopyable>
    (ss.str().c_str(), init<>())
    .def("__getitem__", &HBSplinesFESpace_GetItem<TDim>)
    .def("GetBoundaryBfs", &HBSplinesFESpace_ExtractBoundaryBfsByFlag<TDim>) // deprecated
    .def("ExtractBoundaryBfsByFlag", &HBSplinesFESpace_ExtractBoundaryBfsByFlag<TDim>)
//    .def("ConstructBoundaryFESpace", pointer_to_ConstructBoundaryFESpace1)
    // .def("ConstructBoundaryFESpace", pointer_to_ConstructBoundaryFESpace2)
    .def("UpdateCells", &HBSplinesFESpace<TDim>::UpdateCells)
    .def("MaxLevel", &HBSplinesFESpace_MaxLevel<TDim>)
    .def("SetMaxLevel", &HBSplinesFESpace<TDim>::SetMaxLevel)
    .def("GetBfByEquationId", &HBSplinesFESpace<TDim>::pGetBfByEquationId)
    .def("HasBfByEquationId", &HBSplinesFESpace<TDim>::HasBfByEquationId)
    .def("HasBfById", &HBSplinesFESpace<TDim>::HasBfById)
    .def(self_ns::str(self))
    ;

    IsogeometricApplication_AddPointBasedControlGrid_Helper<Variable<double>, HBSplinesFESpace<TDim> >::Execute();
    IsogeometricApplication_AddPointBasedControlGrid_Helper<Variable<array_1d<double, 3> >, HBSplinesFESpace<TDim> >::Execute();
    IsogeometricApplication_AddPointBasedControlGrid_Helper<Variable<Vector>, HBSplinesFESpace<TDim> >::Execute();

    ////////////////////OLD H-SPLINES////////////////////////

    ss.str(std::string());
    ss << "DeprecatedHBMesh" << TDim << "D";
    class_<DeprecatedHBMesh<TDim>, bases<Patch<TDim> > >
    // class_<DeprecatedHBMesh<TDim>, typename DeprecatedHBMesh<TDim>::Pointer, bases<Patch<TDim> > >
    (ss.str().c_str(), init<const std::size_t&, const std::string&>())
    .def("SetEchoLevel", &DeprecatedHBMesh<TDim>::SetEchoLevel)
    .def("ReadMesh", &DeprecatedHBMesh<TDim>::ReadMesh)
    /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
    .def("Refine", &DeprecatedHBMesh<TDim>::Refine) // use this for debugging only, use RefineNodes and LinearDependencyRefine instead
    .def("RefineNodes", &DeprecatedHBMesh<TDim>::RefineNodes)
    .def("LinearDependencyRefine", &DeprecatedHBMesh<TDim>::LinearDependencyRefine)
    .def("BuildMesh", &DeprecatedHBMesh<TDim>::BuildMesh)
    /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
    .def("ExportCellTopology", &DeprecatedHBMesh<TDim>::ExportCellTopology)
    .def("ExportCellGeology", &DeprecatedHBMesh<TDim>::ExportCellGeology)
    //    .def("ExportRefinedDomain", &DeprecatedHBMesh<TDim>::ExportRefinedDomain)
    .def("ExportSupportDomain", &DeprecatedHBMesh<TDim>::ExportSupportDomain)
    .def("ExportMatlab", &DeprecatedHBMesh<TDim>::ExportMatlab)
    /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
    .def("ExportMDPA", &DeprecatedHBMesh<TDim>::ExportMDPA)
    .def("ExportMDPA2", &DeprecatedHBMesh<TDim>::ExportMDPA2)
    .def("ExportPostMDPA", &DeprecatedHBMesh<TDim>::ExportPostMDPA)
    .def("ExportCellGeologyAsPostMDPA", &DeprecatedHBMesh<TDim>::ExportCellGeologyAsPostMDPA)
    /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
    .def("PrintKnotVectors", &DeprecatedHBMesh<TDim>::PrintKnotVectors)
    .def("PrintCells", &DeprecatedHBMesh<TDim>::PrintCells)
    .def("PrintBasisFuncs", &DeprecatedHBMesh<TDim>::PrintBasisFuncs)
    .def("PrintRefinementHistory", &DeprecatedHBMesh<TDim>::PrintRefinementHistory)
    .def("CheckNestedSpace", &DeprecatedHBMesh<TDim>::CheckNestedSpace)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "DeprecatedHBMesh" << TDim << "DPointer";
    class_<typename DeprecatedHBMesh<TDim>::Pointer>
    (ss.str().c_str(), init<typename DeprecatedHBMesh<TDim>::Pointer>())
    .def("GetReference", GetReference<DeprecatedHBMesh<TDim> >, return_value_policy<reference_existing_object>())
    .def(self_ns::str(self))
    ;

}

////////////////////////////////////////

void IsogeometricApplication_AddHBSplinesToPython()
{

    /////////////////////////////////////////////////////////////////
    ///////////////////////HIERARCHICAL BSplines/////////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddHBSplinesSpaceToPython<1>();
    IsogeometricApplication_AddHBSplinesSpaceToPython<2>();
    IsogeometricApplication_AddHBSplinesSpaceToPython<3>();

    class_<HBSplinesPatchUtility, HBSplinesPatchUtility::Pointer, boost::noncopyable>
    ("HBSplinesPatchUtility", init<>())
    .def("CreatePatchFromBSplines", &HBSplinesPatchUtility_CreatePatchFromBSplines<2>)
    .def("CreatePatchFromBSplines", &HBSplinesPatchUtility_CreatePatchFromBSplines<3>)
    .def("ListBoundaryBfs", &HBSplinesPatchUtility_ListBoundaryBfs<2>)
    .def("ListBoundaryBfs", &HBSplinesPatchUtility_ListBoundaryBfs<3>)
    .def("GetBfByEquationId", &HBSplinesPatchUtility_GetBfByEquationId<2>)
    .def("GetBfByEquationId", &HBSplinesPatchUtility_GetBfByEquationId<3>)
    .def("ReportDuplicatedEquationId", &HBSplinesPatchUtility_ReportDuplicatedEquationId<2>)
    .def("ReportDuplicatedEquationId", &HBSplinesPatchUtility_ReportDuplicatedEquationId<3>)
    .def(self_ns::str(self))
    ;

    class_<HBSplinesRefinementUtility, typename HBSplinesRefinementUtility::Pointer, boost::noncopyable>
    ("HBSplinesRefinementUtility", init<>())
    .def("Refine", &HBSplinesRefinementUtility_Refine<2>)
    .def("Refine", &HBSplinesRefinementUtility_Refine<3>)
    .def("Refine", &HBSplinesRefinementUtility_RefineBf<2>)
    .def("Refine", &HBSplinesRefinementUtility_RefineBf<3>)
    .def("RefineWindow", &HBSplinesRefinementUtility_RefineWindow<2>)
    .def("RefineWindow", &HBSplinesRefinementUtility_RefineWindow<3>)
    .def("LinearDependencyRefine", &HBSplinesRefinementUtility_LinearDependencyRefine<2>)
    .def("LinearDependencyRefine", &HBSplinesRefinementUtility_LinearDependencyRefine<3>)
    .def(self_ns::str(self))
    ;

    class_<MultiHBSplinesPatchMatlabExporter, MultiHBSplinesPatchMatlabExporter::Pointer, boost::noncopyable>
    ("MultiHBSplinesPatchMatlabExporter", init<>())
    .def("Export", &MultiPatchExporter_Export<1, MultiHBSplinesPatchMatlabExporter, Patch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiHBSplinesPatchMatlabExporter, Patch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiHBSplinesPatchMatlabExporter, Patch<3> >)
    .def("Export", &MultiPatchExporter_Export<1, MultiHBSplinesPatchMatlabExporter, MultiPatch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiHBSplinesPatchMatlabExporter, MultiPatch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiHBSplinesPatchMatlabExporter, MultiPatch<3> >)
    .def(self_ns::str(self))
    ;

}

}  // namespace Python.

} // Namespace Kratos