/*    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.
 *
 *    References
 *      Mooij, E. The Motion of a Vehicle in a Planetary Atmosphere, TU Delft, 1997.
 *
 *    Notes
 *      The reference frame definitions/abbreviations can be found in the file
 *      referenceFrameTransformations.h.
 *
 */

#define BOOST_TEST_MAIN

#include <iostream>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

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

#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"
#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"

namespace tudat
{
namespace unit_tests
{

using namespace unit_conversions;

BOOST_AUTO_TEST_SUITE( test_reference_frame_transformations )

// Summary of tests.
// Test 1: Test inertial to rotating planetocentric frame transformation.
// Test 2: Check whether the transformed matrix of Test 1 is also correct.
// Test 3: Same test as Test 1 for the transformation quaternion.
// Test 4: Same test as Test 2 for the transformation quaternion.
// Test 5: Test airspeed-based aerodynamic to body frame transformation.
// Test 6: Test planetocentric to local vertical frame transformation quaternion.
// Test 7: Check whether the transformed matrix of Test 6 is also correct.
// Test 8: Test transformations between trajectory and local vertical frame.
// Test 9: Test transformations between trajectory and aerodynamic frame.
// Test 10: Test transformations between body and aerodynamic reference frame.

// Test inertial to rotating planetocentric frame transformations.
BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricFrameTransformations )
{
    // Test 1: Test Inertial to rotating planetocentric frame transformation.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        double horizontalStartLocationSize = sqrt( std::pow( startLocation( 0 ), 2.0 )
                                                   + std::pow( startLocation( 1 ), 2.0 ) );

        // Declare and initialize angle between vector and XR-axis.
        double startAngle = std::atan2( startLocation( 1 ), startLocation( 0 ) );

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Declare and initialize the angle between the XI- and XR-axis.
        double endAngle = startAngle - angleInTime;

        // Declare the expected location of the point in the planetocentric reference frame.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = horizontalStartLocationSize * std::cos( endAngle );
        expectedLocation( 1 ) = horizontalStartLocationSize * std::sin( endAngle );
        expectedLocation( 2 ) = startLocation( 2 );

        // Compute location of the point in the rotating frame subject to the transformation
        // matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getInertialToPlanetocentricFrameTransformationMatrix( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: Check whether the transformed matrix of Test 1 is also correct.
    // Compute the error in the calculation.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getRotatingPlanetocentricToInertialFrameTransformationMatrix( angleInTime ) *
                reference_frames::
                getInertialToPlanetocentricFrameTransformationMatrix( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, startLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricFrameTransformationQuaternion )
{

    // Test 3: Same test as Test 1 for the transformation quaternion.
    // Compute location of the point in the Rotating frame subject to the transformation matrix.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        double horizontalStartLocationSize = sqrt( std::pow( startLocation( 0 ), 2.0 )
                                                   + std::pow( startLocation( 1 ), 2.0 ) );

        // Declare and initialize angle between vector and XR-axis.
        double startAngle = std::atan2( startLocation( 1 ), startLocation( 0 ) );

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Declare and initialize the angle between the XI- and XR-axis.
        double endAngle = startAngle - angleInTime;

        // Declare the expected location of the point in the planetocentric reference frame.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = horizontalStartLocationSize * std::cos( endAngle );
        expectedLocation( 1 ) = horizontalStartLocationSize * std::sin( endAngle );
        expectedLocation( 2 ) = startLocation( 2 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getInertialToPlanetocentricFrameTransformationQuaternion( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: Same test as Test 2 for the transformation quaternion.
    // Compute the error in the calculation.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 4.0;
        startLocation( 1 ) = 1.0;
        startLocation( 2 ) = 5.0;

        // Rotate by 10 degrees around the positive Z-axis
        double angleInTime = convertDegreesToRadians( 10.0 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getRotatingPlanetocentricToInertialFrameTransformationQuaternion( angleInTime ) *
                reference_frames::
                getInertialToPlanetocentricFrameTransformationQuaternion( angleInTime )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, startLocation,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_CASE( testAirspeedBasedAerodynamicToBodyFrameTransformation )
{
    // Test 5: Test airspeed-Based Aerodynamic to body frame transformation.
    // Declare and initialize the start location and angles.
    {
        // Initialize initial location vector in inertial frame.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = -10.0;
        startLocation( 1 ) = 0.0;
        startLocation( 2 ) = 0.0;

        double angleOfAttack = convertDegreesToRadians( 45.0 );
        double angleOfSideslip = convertDegreesToRadians( 60.0 );;

        // Compute expected location.
        // As there is only an angle of attack, the following simplified equations can be used.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = startLocation( 0 ) * std::cos( angleOfSideslip ) * std::cos( angleOfAttack );
        expectedLocation( 1 ) = startLocation( 0 ) * std::sin( angleOfSideslip );
        expectedLocation( 2 ) = startLocation( 0 ) * std::cos( angleOfSideslip ) * std::sin( angleOfAttack );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix(
                    angleOfAttack, angleOfSideslip ) * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation, 1.0e-14 );
    }
}

BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion )
{
    // Test 6: Test Rotating planetocentric to local vertical frame transformation quaternion.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angle for planet and set to 90 degrees.
        double longitude = convertDegreesToRadians( 60.0 );

        // Declare latitude and set to 45 degrees.
        double latitude = convertDegreesToRadians( 20.0 );

        // Declare the expected location of the point in the planet reference frame.
        Eigen::Vector3d expectedLocation;
        expectedLocation( 0 ) = -std::cos( longitude ) * std::sin( latitude ) * startLocation( 0 ) -
                std::sin( longitude ) * std::sin( latitude ) * startLocation( 1 )  +
                std::cos( latitude ) * startLocation( 2 );
        expectedLocation( 1 ) = -std::sin( longitude ) * startLocation( 0 ) +
                std::cos( longitude ) * startLocation( 1 ) +
                0.0;
        expectedLocation( 2 ) = -std::cos( longitude ) * std::cos( latitude ) * startLocation( 0 ) -
                std::sin( longitude ) * std::cos( latitude ) * startLocation( 1 )  -
                std::sin( latitude ) * startLocation( 2 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion(
                    longitude, latitude ) * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, expectedLocation, 1.0e-14 );
    }

    // Test 7: Check whether the transformed matrix of Test 6 is also correct.
    // Compute the error in the calculation.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angle for planet and set to 90 degrees.
        double longitude = convertDegreesToRadians( 60.0 );

        // Declare latitude and set to 45 degrees.
        double latitude = convertDegreesToRadians( 20.0 );

        // Compute location of the point in the rotating frame subject to the transformation matrix.
        Eigen::Vector3d transformedLocation;
        transformedLocation = reference_frames::
                getLocalVerticalToRotatingPlanetocentricFrameTransformationQuaternion( longitude,
                                                                                latitude )
                * reference_frames::
                getRotatingPlanetocentricToLocalVerticalFrameTransformationQuaternion( longitude,
                                                                                latitude )
                * startLocation;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocation, startLocation, 1.0e-14 );
    }
}

// Test inertial to rotating planetocentric frame transformations, without assumption of equal x-y
// planes.
BOOST_AUTO_TEST_CASE( testRotatingPlanetocentricWithEquatorChangeFrameTransformations )
{
    using namespace reference_frames;

    // Data from pck00010.tpc Spice kernel.
    const double venusPoleRightAscension = convertDegreesToRadians( 272.76 );
    const double venusPoleDeclination = convertDegreesToRadians( 67.16 );
    const double venusPrimeMeridianAtJ2000 = convertDegreesToRadians( 160.20 );

    // Set rotation, as calculated with Spice
    // (see Astrodynamics/Ephemerides/unitTestSimpleRotationalEphemeris.cpp).
    Eigen::Matrix3d spiceInitialRotationToTargetFrameMatrix;
    spiceInitialRotationToTargetFrameMatrix
            << -0.9548214974296336, 0.2665104385944917, 0.1314841974018291,
            -0.296591573568662, -0.882413772579987, -0.3652114078848295,
            0.01869081416890202, -0.3877088083617989, 0.9215923900425705;

    // Calculate given rotation to Venus-fixed frame.
    Eigen::Matrix3d calculatedTransformationMatrix = Eigen::Matrix3d(
                getInertialToPlanetocentricFrameTransformationQuaternion(
                    venusPoleDeclination, venusPoleRightAscension, venusPrimeMeridianAtJ2000 ) );

    // Compare calculated with Spice result.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                spiceInitialRotationToTargetFrameMatrix, calculatedTransformationMatrix, 1.0E-15 );

    // Calculate inverse rotation.
    calculatedTransformationMatrix = Eigen::Matrix3d(
                getRotatingPlanetocentricToInertialFrameTransformationQuaternion(
                    venusPoleDeclination, venusPoleRightAscension,
                    venusPrimeMeridianAtJ2000 ) );

    // Multiply inverse rotation with Spice rotation to Venus-fixed frame. The two should be
    // orthonormal, so result should be the identity matrix.
    Eigen::Matrix3d matrixToCheck =
            spiceInitialRotationToTargetFrameMatrix * calculatedTransformationMatrix;

    // Check whether result is indeed (close to) identity matrix.
    for ( unsigned int i = 0; i < 3; i++ )
    {
        for ( unsigned int j = 0; j < 3; j++ )
        {
            if ( i == j )
            {
                BOOST_CHECK_CLOSE_FRACTION( matrixToCheck( i, j ), 1.0, 1.0E-15 );
            }
            else
            {
                BOOST_CHECK_SMALL( matrixToCheck( i, j ), 1.0E-15 );
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( testTrajectoryToLocalVerticalFrameTransformations )
{
    // Test 8: Test trajectory to local vertical frame and inverse transformations.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angles.
        for ( double headingAngle = convertDegreesToRadians( 0.0 );
              headingAngle < convertDegreesToRadians( 360.0 );
              headingAngle += convertDegreesToRadians( 30.0 ) )
        {
            for ( double flightPathAngle = convertDegreesToRadians( -90.0 );
                  flightPathAngle < convertDegreesToRadians( 90.0 );
                  flightPathAngle += convertDegreesToRadians( 30.0 ) )
            {
                // Declare the expected location of the point in the V-frame (per Mooij 1997).
                Eigen::Vector3d expectedLocation;
                Eigen::Matrix3d rotation;
                rotation << std::cos( headingAngle ) * std::cos( flightPathAngle ),
                        -std::sin( headingAngle ),
                        std::cos( headingAngle ) * std::sin( flightPathAngle ),
                        std::sin( headingAngle ) * std::cos( flightPathAngle ),
                        std::cos( headingAngle ),
                        std::sin( headingAngle ) * std::sin( flightPathAngle ),
                        -std::sin( flightPathAngle ),
                        0.0,
                        std::cos( flightPathAngle );
                expectedLocation = rotation * startLocation;

                // Compute location of the point in the V-frame using the tested function.
                Eigen::Vector3d transformedLocationQuat =
                        reference_frames::
                        getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                            flightPathAngle, headingAngle ) * startLocation;
                Eigen::Vector3d transformedLocationMat =
                        reference_frames::getTrajectoryToLocalVerticalFrameTransformationMatrix(
                            flightPathAngle, headingAngle ) * startLocation;

                // Compute product of inverse rotations.
                Eigen::Vector3d inverseRotationProductMat =
                        reference_frames::getTrajectoryToLocalVerticalFrameTransformationMatrix(
                            flightPathAngle, headingAngle ) *
                        reference_frames::getLocalVerticalFrameToTrajectoryTransformationMatrix(
                            flightPathAngle, headingAngle ) * startLocation;
                Eigen::Vector3d inverseRotationProductQuat =
                        reference_frames::
                        getTrajectoryToLocalVerticalFrameTransformationQuaternion(
                            flightPathAngle, headingAngle ) *
                        reference_frames::
                        getLocalVerticalFrameToTrajectoryTransformationQuaternion(
                            flightPathAngle, headingAngle ) * startLocation;

                // Check whether transformed vectors match within tolerances.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            transformedLocationQuat, expectedLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            transformedLocationMat, expectedLocation, 2.0e-14 );

                // Check whether product of inverse rotations equals unity.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            inverseRotationProductMat, startLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            inverseRotationProductQuat, startLocation, 1.0e-14 );
            }
        }
    }
}

BOOST_AUTO_TEST_CASE( testTrajectoryToAerodynamicFrameTransformations )
{
    // Test 9: Test trajectory to aerodynamic and inverse transformations.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angles.
        for ( double bankAngle = convertDegreesToRadians( 0.0 );
              bankAngle < convertDegreesToRadians( 360.0 );
              bankAngle += convertDegreesToRadians( 30.0 ) )
        {
            // Declare the expected location of the point in the aerodynamic reference frame
            // (per Mooij 1997).
            Eigen::Vector3d expectedLocation;
            Eigen::Matrix3d rotation;
            rotation << 1.0, 0.0, 0.0,
                    0.0, std::cos( bankAngle ), -std::sin( bankAngle ),
                    0.0, std::sin( bankAngle ), std::cos( bankAngle );
            expectedLocation = rotation * startLocation;

            // Compute location of the point in the rotating frame subject to the
            // transformation matrix.
            Eigen::Vector3d transformedLocationQuat =
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationQuaternion(
                        bankAngle ) * startLocation;
            Eigen::Vector3d transformedLocationMat =
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationMatrix(
                        bankAngle ) * startLocation;

            // Compute product of inverse rotations.
            Eigen::Vector3d inverseRotationProductMat =
                    reference_frames::getAerodynamicToTrajectoryFrameTransformationMatrix(
                        bankAngle ) *
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationMatrix(
                        bankAngle ) * startLocation;
            Eigen::Vector3d inverseRotationProductQuat =
                    reference_frames::getAerodynamicToTrajectoryFrameTransformationQuaternion(
                        bankAngle ) *
                    reference_frames::getTrajectoryToAerodynamicFrameTransformationQuaternion(
                        bankAngle ) * startLocation;

            // Check whether transformed vectors match within tolerances.
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        transformedLocationQuat, expectedLocation, 1.0e-14 );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transformedLocationMat, expectedLocation, 1.0e-14 );

            // Check whether product of inverse rotations equals unity.
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION( inverseRotationProductMat, startLocation, 1.0e-14 );
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        inverseRotationProductQuat, startLocation, 1.0e-14 );
        }
    }
}

BOOST_AUTO_TEST_CASE( testAerodynamicToBodyFrameTransformations )
{
    // Test 10: Test body to airspeed-based aerodynamic frame and inverse transformations.
    {
        // Initialize initial location vector.
        Eigen::Vector3d startLocation;
        startLocation( 0 ) = 10.0;
        startLocation( 1 ) = 5.0;
        startLocation( 2 ) = 2.0;

        // Declare rotation angles.
        for ( double angleOfAttack = convertDegreesToRadians( -180.0 );
              angleOfAttack < convertDegreesToRadians( 180.0 );
              angleOfAttack += convertDegreesToRadians( 30.0 ) )
        {
            for ( double angleOfSideslip = convertDegreesToRadians( -90.0 );
                  angleOfSideslip < convertDegreesToRadians( 90.0 );
                  angleOfSideslip += convertDegreesToRadians( 30.0 ) )
            {
                // Declare the expected location of the point in the AA-frame.
                // (per Mooij 1997).
                Eigen::Vector3d expectedLocation;
                Eigen::Matrix3d rotation;
                rotation << std::cos( angleOfAttack ) * std::cos( angleOfSideslip ),
                        std::sin( angleOfSideslip ),
                        std::sin( angleOfAttack ) * std::cos( angleOfSideslip ),
                        -std::cos( angleOfAttack ) * std::sin( angleOfSideslip ),
                        std::cos( angleOfSideslip ),
                        -std::sin( angleOfAttack ) * std::sin( angleOfSideslip ),
                        -std::sin( angleOfAttack ),
                        0.0,
                        std::cos( angleOfAttack );
                expectedLocation = rotation * startLocation;

                // Compute location of the point in the AA-frame using the tested function.
                Eigen::Vector3d transformedLocationQuat =
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                            angleOfAttack, angleOfSideslip ) * startLocation;
                Eigen::Vector3d transformedLocationMat =
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationMatrix(
                            angleOfAttack, angleOfSideslip ) * startLocation;

                // Compute product of inverse rotations
                Eigen::Vector3d inverseRotationProductMat =
                        reference_frames::
                        getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix(
                            angleOfAttack, angleOfSideslip ) *
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationMatrix(
                            angleOfAttack, angleOfSideslip ) * startLocation;
                Eigen::Vector3d inverseRotationProductQuat =
                        reference_frames::
                        getAirspeedBasedAerodynamicToBodyFrameTransformationQuaternion(
                            angleOfAttack, angleOfSideslip ) *
                        reference_frames::
                        getBodyToAirspeedBasedAerodynamicFrameTransformationQuaternion(
                            angleOfAttack, angleOfSideslip ) * startLocation;

                // Check whether transformed vectors match within tolerances.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            transformedLocationQuat, expectedLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            transformedLocationMat, expectedLocation, 1.0e-14 );

                // Check whether product of inverse rotations equals unity.
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            inverseRotationProductMat, startLocation, 1.0e-14 );
                TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                            inverseRotationProductQuat, startLocation, 1.0e-14 );
            }
        }
    }
}

//! Function to test if a matrix is an identity matrix.
void testIsIdentityMatrix( const Eigen::Matrix3d matrixToTest )
{
    for( unsigned int i = 0; i < 3; i++ )
    {
        for( unsigned int j = 0; j < 3; j++ )
        {
            if( i == j )
            {
                BOOST_CHECK_CLOSE_FRACTION( matrixToTest( i, j ), 1.0, std::numeric_limits< double >::epsilon( ) );
            }
            else
            {
                BOOST_CHECK_SMALL( std::fabs( matrixToTest( i, j ) ), std::numeric_limits< double >::epsilon( ) );
            }
        }
    }
}

//! Function to test if a rotation matrix defines a right-handed frame.
void testIsRotationMatrixRightHanded( const Eigen::Matrix3d matrixToTest )
{
    Eigen::Vector3d expectedUnitVectorZ =
            ( Eigen::Vector3d( matrixToTest.block( 0, 0, 3, 1 ) ) ).cross(
                Eigen::Vector3d( matrixToTest.block( 0, 1, 3, 1 ) ) );
    Eigen::Vector3d expectedUnitVectorX =
            ( Eigen::Vector3d( matrixToTest.block( 0, 1, 3, 1 ) ) ).cross(
                Eigen::Vector3d( matrixToTest.block( 0, 2, 3, 1 ) ) );
    Eigen::Vector3d expectedUnitVectorY  =
            ( Eigen::Vector3d( matrixToTest.block( 0, 2, 3, 1 ) ) ).cross(
                Eigen::Vector3d( matrixToTest.block( 0, 0, 3, 1 ) ) );

    for( unsigned int i = 0; i < 3; i++ )\
    {
        BOOST_CHECK_CLOSE_FRACTION(
                    expectedUnitVectorX( i ), matrixToTest( i, 0 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION(
                    expectedUnitVectorY( i ), matrixToTest( i, 1 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_CLOSE_FRACTION(
                    expectedUnitVectorZ( i ), matrixToTest( i, 2 ), 2.0 * std::numeric_limits< double >::epsilon( ) );
    }
}

// Test velocity based LVLH frame transformation.
BOOST_AUTO_TEST_CASE( testVelocityBasedLvlhFrameTransformations )
{
    // Test 11: Test velocity=based LVLH frame to inertial (I) frame transformation: for arbitrary state vectors,
    // test all expected properties of rotation matrices
    {
        // Position vectors to semi-random values
        Eigen::Vector6d vehicleStateCartesian, centralBodyStateCartesian, relativeState;
        vehicleStateCartesian << 3.2, -1.7, 8.2, -1.4E-3, -5.6E-4, 9.2E-4;
        centralBodyStateCartesian << 7.4, 6.3, -3.6, 5.3E-4, 7.64E3, -4.4E-4;

        Eigen::Matrix3d nAxisAwayFromBodyMatrix = reference_frames::getVelocityBasedLvlhToInertialRotation(
                    vehicleStateCartesian, centralBodyStateCartesian, true );
        Eigen::Matrix3d nAxisAwayTowardsBodyMatrix = reference_frames::getVelocityBasedLvlhToInertialRotation(
                    vehicleStateCartesian, centralBodyStateCartesian, false );

        // Test if central body is properly processed.
        {
            Eigen::Matrix3d nAxisAwayFromBodyMatrixCentral = reference_frames::getVelocityBasedLvlhToInertialRotation(
                        vehicleStateCartesian - centralBodyStateCartesian, Eigen::Vector6d::Zero( ), true );
            Eigen::Matrix3d nAxisAwayTowardsBodyMatrixCentral = reference_frames::getVelocityBasedLvlhToInertialRotation(
                        vehicleStateCartesian - centralBodyStateCartesian, Eigen::Vector6d::Zero( ), false );

            for( unsigned int i = 0; i < 3; i++ )
            {
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_CLOSE_FRACTION(
                                nAxisAwayFromBodyMatrixCentral( i, j ), nAxisAwayFromBodyMatrix( i, j ),
                                2.0 * std::numeric_limits< double >::epsilon( ) );
                    BOOST_CHECK_CLOSE_FRACTION(
                                nAxisAwayTowardsBodyMatrix( i, j ), nAxisAwayTowardsBodyMatrixCentral( i, j ),
                                2.0 * std::numeric_limits< double >::epsilon( ) );
                }

            }
        }

        // Compute Relative states
        relativeState = vehicleStateCartesian - centralBodyStateCartesian;

        // Test if n axis indeed points away from body
        double positionDotProductWithNAwayFromBody =
                Eigen::Vector3d( nAxisAwayFromBodyMatrix.block( 0, 1, 3, 1 ) ).dot(
                    relativeState.segment( 0, 3 ).normalized( ) );
        BOOST_CHECK_EQUAL( positionDotProductWithNAwayFromBody > 0, 1 );

        // Test if n axis indeed points towards body
        double positionDotProuctWithNTowardsBody =
                Eigen::Vector3d( nAxisAwayTowardsBodyMatrix.block( 0, 1, 3, 1 ) ).dot(
                    relativeState.segment( 0, 3 ).normalized( ) );
        BOOST_CHECK_EQUAL( positionDotProuctWithNTowardsBody < 0, 1 );

        // Check if axes point exactly in opposite directions
        BOOST_CHECK_CLOSE_FRACTION( positionDotProductWithNAwayFromBody, -positionDotProuctWithNTowardsBody,
                                    std::numeric_limits< double >::epsilon( ) );

        // Test if two matrices compare as they should
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_CLOSE_FRACTION(
                            nAxisAwayFromBodyMatrix( i, j ), ( j == 0 ? 1.0 : -1.0 ) * nAxisAwayTowardsBodyMatrix( i, j ),
                            2.0 * std::numeric_limits< double >::epsilon( ) );
            }
        }

        // Test orthonormality of matrices
        Eigen::Matrix3d matrixProduct;
        matrixProduct = nAxisAwayFromBodyMatrix * nAxisAwayFromBodyMatrix.transpose( );
        testIsIdentityMatrix( matrixProduct );

        matrixProduct = nAxisAwayTowardsBodyMatrix * nAxisAwayTowardsBodyMatrix.transpose( );
        testIsIdentityMatrix( matrixProduct );

        matrixProduct = nAxisAwayFromBodyMatrix * nAxisAwayFromBodyMatrix.inverse( );
        testIsIdentityMatrix( matrixProduct );

        matrixProduct = nAxisAwayTowardsBodyMatrix * nAxisAwayTowardsBodyMatrix.inverse( );
        testIsIdentityMatrix( matrixProduct );

        // Test if matrices define right-handed frames
        testIsRotationMatrixRightHanded( nAxisAwayFromBodyMatrix );
        testIsRotationMatrixRightHanded( nAxisAwayTowardsBodyMatrix );

        Eigen::Vector3d lvlhFrameThrust1, lvlhFrameThrust2;

        // Define semi-random thrust vector along velocity vector
        double thrustScale = 25.2;
        Eigen::Vector3d inertialThrustVectorAlongVelocity = thrustScale * relativeState.segment( 3, 3 );
        {
            // Compute local thrust vectors
            lvlhFrameThrust1 =
                    nAxisAwayFromBodyMatrix.transpose( ) * inertialThrustVectorAlongVelocity;
            lvlhFrameThrust2 =
                    nAxisAwayTowardsBodyMatrix.transpose( ) * inertialThrustVectorAlongVelocity;

            // Compute thrust vector magnitudes
            double thrustMagnitudeAlongVelocityVector = inertialThrustVectorAlongVelocity.norm( );

            // Test if entry 0 of two vectors are the same
            BOOST_CHECK_CLOSE_FRACTION( lvlhFrameThrust1( 0 ), lvlhFrameThrust2( 0 ),
                                        std::numeric_limits< double >::epsilon( ) );

            // Test thrust vector norms.
            BOOST_CHECK_CLOSE_FRACTION( thrustMagnitudeAlongVelocityVector, lvlhFrameThrust1.norm( ),
                                        5.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( thrustMagnitudeAlongVelocityVector, lvlhFrameThrust2.norm( ),
                                        5.0 * std::numeric_limits< double >::epsilon( ) );

            // Test if entry 0 magnitude is equal to norm
            BOOST_CHECK_CLOSE_FRACTION( std::fabs( lvlhFrameThrust1( 0 ) ), thrustMagnitudeAlongVelocityVector,
                                        std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( std::fabs( lvlhFrameThrust2( 0 ) ), thrustMagnitudeAlongVelocityVector,
                                        std::numeric_limits< double >::epsilon( ) );

            // Test if other entries are 0 (to numerical precision).
            BOOST_CHECK_SMALL( lvlhFrameThrust1( 1 ),
                               thrustMagnitudeAlongVelocityVector * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL( lvlhFrameThrust2( 1 ),
                               thrustMagnitudeAlongVelocityVector * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL( lvlhFrameThrust1( 2 ),
                               thrustMagnitudeAlongVelocityVector * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL( lvlhFrameThrust2( 2 ),
                               thrustMagnitudeAlongVelocityVector * std::numeric_limits< double >::epsilon( ) );
        }

        // Define semi-random thrust vector perpendicular to plane
        Eigen::Vector3d inertialThrustPerpendicularToPlane = thrustScale *  (
                    Eigen::Vector3d( relativeState.segment( 0, 3 ) ).cross(
                        Eigen::Vector3d( relativeState.segment( 3, 3 ) ) ) );
        {
            // Compute local thrust vectors
            lvlhFrameThrust1 =
                    nAxisAwayFromBodyMatrix.transpose( ) * inertialThrustPerpendicularToPlane;
            lvlhFrameThrust2 =
                    nAxisAwayTowardsBodyMatrix.transpose( ) * inertialThrustPerpendicularToPlane;

            // Compute thrust vector magnitudes
            double thrustMagnitudePerpendicularToPlane = inertialThrustPerpendicularToPlane.norm( );

            // Test if entry 2 of two vectors are the same but opposite
            BOOST_CHECK_CLOSE_FRACTION( lvlhFrameThrust1( 2 ), -lvlhFrameThrust2( 2 ),
                                        std::numeric_limits< double >::epsilon( ) );

            // Test thrust vector norms.
            BOOST_CHECK_CLOSE_FRACTION( thrustMagnitudePerpendicularToPlane, lvlhFrameThrust1.norm( ),
                                        5.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( thrustMagnitudePerpendicularToPlane, lvlhFrameThrust2.norm( ),
                                        5.0 * std::numeric_limits< double >::epsilon( ) );

            // Test if entry 2 magnitude is equal to norm
            BOOST_CHECK_CLOSE_FRACTION( std::fabs( lvlhFrameThrust1( 2 ) ), thrustMagnitudePerpendicularToPlane,
                                        std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( std::fabs( lvlhFrameThrust2( 2 ) ), thrustMagnitudePerpendicularToPlane,
                                        std::numeric_limits< double >::epsilon( ) );

            // Test if other entries are 0 (to numerical precision).
            BOOST_CHECK_SMALL( lvlhFrameThrust1( 0 ),
                               thrustMagnitudePerpendicularToPlane * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL( lvlhFrameThrust2( 0 ),
                               thrustMagnitudePerpendicularToPlane * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL( lvlhFrameThrust1( 1 ),
                               thrustMagnitudePerpendicularToPlane * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_SMALL( lvlhFrameThrust2( 1 ),
                               thrustMagnitudePerpendicularToPlane * std::numeric_limits< double >::epsilon( ) );
        }

        // Define semi-random thrust vector along local y-axis
        Eigen::Vector3d inertialThrustAlongLocalYAxis = inertialThrustPerpendicularToPlane.cross(
                    inertialThrustVectorAlongVelocity );
        {
            // Compute local thrust vectors
            lvlhFrameThrust1 =
                    nAxisAwayFromBodyMatrix.transpose( ) * inertialThrustAlongLocalYAxis;
            lvlhFrameThrust2 =
                    nAxisAwayTowardsBodyMatrix.transpose( ) * inertialThrustAlongLocalYAxis;

            // Compute thrust vector magnitudes
            double thrustMagnitudeAlongYAxis = inertialThrustAlongLocalYAxis.norm( );

            // Test if entry 1 of two vectors are the same
            BOOST_CHECK_CLOSE_FRACTION( lvlhFrameThrust1( 1 ), -lvlhFrameThrust2( 1 ),
                                        std::numeric_limits< double >::epsilon( ) );

            // Test thrust vector norms.
            BOOST_CHECK_CLOSE_FRACTION( thrustMagnitudeAlongYAxis, lvlhFrameThrust1.norm( ),
                                        5.0 * std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( thrustMagnitudeAlongYAxis, lvlhFrameThrust2.norm( ),
                                        5.0 * std::numeric_limits< double >::epsilon( ) );

            // Test if entry 1 magnitude is equal to norm
            BOOST_CHECK_CLOSE_FRACTION( std::fabs( lvlhFrameThrust1( 1 ) ), thrustMagnitudeAlongYAxis,
                                        std::numeric_limits< double >::epsilon( ) );
            BOOST_CHECK_CLOSE_FRACTION( std::fabs( lvlhFrameThrust2( 1 ) ), thrustMagnitudeAlongYAxis,
                                        std::numeric_limits< double >::epsilon( ) );

            // Test if other entries are 0 (to numerical precision).
            BOOST_CHECK_SMALL( lvlhFrameThrust1( 0 ), thrustMagnitudeAlongYAxis * std::numeric_limits< double >::epsilon( ));
            BOOST_CHECK_SMALL( lvlhFrameThrust2( 0 ), thrustMagnitudeAlongYAxis * std::numeric_limits< double >::epsilon( ));
            BOOST_CHECK_SMALL( lvlhFrameThrust1( 2 ), thrustMagnitudeAlongYAxis * std::numeric_limits< double >::epsilon( ));
            BOOST_CHECK_SMALL( lvlhFrameThrust2( 2 ), thrustMagnitudeAlongYAxis * std::numeric_limits< double >::epsilon( ));
        }

    }

    // Test 11: Test velocity based LVLH frame to inertial (I) frame transformation.
    {
        // Initialize initial thrust vector in velocity based LVLH frame.
        Eigen::Vector3d startThrustVector;
        startThrustVector( 0 ) = 2.0; // T direction
        startThrustVector( 1 ) = 4.0; // N direction
        startThrustVector( 2 ) = 1.0; // W direction
        bool doesNaxisPointAwayFromCentralBody = false;

        Eigen::Vector6d vehicleStateCartesian, centralBodyStateCartesian;
        vehicleStateCartesian << 0.0, -1.0, 0.0, -1.0, -0.5, 0.0;
        centralBodyStateCartesian << 0.0, 2.0, 0.0, -0.5, 0.0, 0.0;

        // Compute angle between positive T axis and negative y axis
        double angle = std::atan2( ( vehicleStateCartesian( 3 ) - centralBodyStateCartesian( 3 ) ),
                              ( vehicleStateCartesian( 4 ) - centralBodyStateCartesian( 4 ) ) );

        // Declare the expected thrust vector in the inertial (I) reference frame.
        Eigen::Vector3d expectedThrustVector;
        expectedThrustVector( 0 ) = startThrustVector( 0 ) * std::sin( angle ) + startThrustVector( 1 ) * std::cos( angle );
        expectedThrustVector( 1 ) = startThrustVector( 0 ) * std::cos( angle ) - startThrustVector( 1 ) * std::sin( angle );
        expectedThrustVector( 2 ) = -startThrustVector( 2 ); // z direction

        // Compute location of the point in the rotating frame subject to the transformation
        // matrix.
        Eigen::Vector3d transformedThrustVector;
        transformedThrustVector = reference_frames::getVelocityBasedLvlhToInertialRotation(
                    vehicleStateCartesian, centralBodyStateCartesian, doesNaxisPointAwayFromCentralBody ) * startThrustVector;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedThrustVector, transformedThrustVector, 1.0e-15 );
    }

    // Test 12: Test velocity based LVLH frame to planetocentric frame transformation with Keplerian elements.
    {
        // Initialize initial thrust vector in velocity based LVLH frame.
        Eigen::Vector3d startThrustVector;
        startThrustVector( 0 ) = 15.0; // T direction
        startThrustVector( 1 ) = -1.0; // N direction
        startThrustVector( 2 ) = 2.0; // W direction

        Eigen::Vector6d vehicleStateKeplerian;
        vehicleStateKeplerian << 1.0, 0.5, convertDegreesToRadians( 60.0 ),
                convertDegreesToRadians( 180.0 ), convertDegreesToRadians( 15.0 ), convertDegreesToRadians( 90.0 );

        // Declare the expected thrust vector in the inertial (I) reference frame. Reference data comes from Matlab file
        Eigen::Vector3d expectedThrustVector;
        expectedThrustVector( 0 ) = 13.959420290978478079; // x direction
        expectedThrustVector( 1 ) = -1.988147005863377672; // y direction
        expectedThrustVector( 2 ) = -5.5840716885526107944; // z direction

        // Compute location of the point in the rotating frame subject to the transformation
        // matrix.
        Eigen::Matrix3d rotationMatrixFromKeplerElements =
                reference_frames::getVelocityBasedLvlhToPlanetocentricRotationKeplerian(
                    vehicleStateKeplerian ).toRotationMatrix( );
        Eigen::Vector3d transformedThrustVector;
        transformedThrustVector = rotationMatrixFromKeplerElements * startThrustVector;

        // Check whether both vectors are equal within tolerances.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedThrustVector, transformedThrustVector,
                                           ( 10.0 * std::numeric_limits< double >::epsilon( ) ) );

        Eigen::Vector6d vehicleStateCartesian =
                orbital_element_conversions::convertKeplerianToCartesianElements(
                    vehicleStateKeplerian, 3.986E14 );

        Eigen::Matrix3d rotationMatrixFromCartesianElements =
                reference_frames::getVelocityBasedLvlhToInertialRotation(
                    vehicleStateCartesian, Eigen::Vector6d::Zero( ), false );

        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL(
                            std::fabs( rotationMatrixFromCartesianElements( i, j ) -
                                       rotationMatrixFromKeplerElements( i, j ) ),
                            5.0 * std::numeric_limits< double >::epsilon( ) );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <boost/math/special_functions/lanczos.hpp>


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

#include <boost/simd/function/any.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/two.hpp>

#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( log_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const & a0) const BOOST_NOEXCEPT
    {
      return musl_(log)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::double_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const & a0) const BOOST_NOEXCEPT
    {
      return musl_(log)(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( log_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::musl_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(23));
        x = if_else(test, x*A0(0x1p23f), x);
      }
#endif
      uiA0 ix = bitwise_cast<uiA0>(x);
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      ix += 0x3f800000 - 0x3f3504f3;
    //ix +=
      k += bitwise_cast<iA0>(ix>>23) - 0x7f;
      ix = (ix&0x007fffff) + 0x3f3504f3;
      x =  bitwise_cast<A0>(ix);
      A0 f = dec(x);
      A0 s = f/(Two<A0>() + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;

      A0 hfsq = Half<A0>()*sqr(f);
      A0 dk = tofloat(k);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ltz(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::musl_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x = a0;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k(0);
      auto isnez = is_nez(a0);

#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(54));
        x = if_else(test, x*A0(0x1p54), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      hx += 0x3ff00000 - 0x3fe6a09e;
      k += bitwise_cast<iA0>(hx>>20) - 0x3ff;
      A0 dk = tofloat(k);
      hx = (hx&0x000fffff) + 0x3fe6a09e;
      x = bitwise_cast<A0>(hx<<32 | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(x))));

      A0 f = dec(x);
      A0 hfsq = Half<A0>()*sqr(f);
      A0 s = f/(Two<A0>() + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0,
                    0x3fd999999997fa04ll,
                    0x3fcc71c51d8e78afll,
                    0x3fc39a09d078c69fll
                    > (w);
      A0 t2= z*horn<A0,
                    0x3fe5555555555593ll,
                    0x3fd2492494229359ll,
                    0x3fc7466496cb03dell,
                    0x3fc2f112df3e5244ll
                    > (w);
      A0 R = t2+t1;
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ltz(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::plain_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(23));
        x = if_else(test, x*A0(0x1p23f), x);
      }
#endif
      iA0 kk;
      std::tie(x, kk) = fast_(frexp)(x);
      A0  x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() > x);
      k += kk+bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      A0 dk = tofloat(k);
      A0 s = f/(Two<A0>() + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;

      A0 hfsq = Half<A0>()*sqr(f);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ltz(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
//      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(23));
        x = if_else(test, x*A0(0x1p23f), x);
      }
#endif
      iA0 kk;
      std::tie(x, kk) = fast_(frexp)(x);
      A0  x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() >  x);
      k += kk+bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      A0 dk = tofloat(k);
      // compute approximation
      A0 s = f/(Two<A0>()+f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0,
        0x3fd999999997fa04ll,
        0x3fcc71c51d8e78afll,
        0x3fc39a09d078c69fll
        > (w);
      A0 t2= z*horn<A0,
        0x3fe5555555555593ll,
        0x3fd2492494229359ll,
        0x3fc7466496cb03dell,
        0x3fc2f112df3e5244ll
        > (w);
      A0 R = t2+t1;
      A0 hfsq = Half<A0>()* sqr(f);
      A0 r = fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()) - hfsq) + f));
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ltz(a0), zz);
    }
  };

} } }

#endif
  /*
   *   1. Argument Reduction: find k and f such that
   *                      x = 2^k * (1+f),
   *         where  sqrt(2)/2 < 1+f < sqrt(2) .
   *
   *   2. Approximation of log(1+f).
   *      Let s = f/(2+f) ; based on log(1+f) = log(1+s) - log(1-s)
   *               = 2s + 2/3 s**3 + 2/5 s**5 + .....,
   *               = 2s + s*R
   *      We use a special Remez algorithm on [0,0.1716] to generate
   *      a polynomial of degree 14 to approximate R The maximum error
   *      of this polynomial approximation is bounded by 2**-58.45. In
   *      other words,
   *                      2      4      6      8      10      12      14
   *          R(z) ~ Lg1*s +Lg2*s +Lg3*s +Lg4*s +Lg5*s  +Lg6*s  +Lg7*s
   *      (the values of Lg1 to Lg7 are listed in the program)
   *      and
   *          |      2          14          |     -58.45
   *          | Lg1*s +...+Lg7*s    -  R(z) | <= 2
   *          |                             |
   *      Note that 2s = f - s*f = f - hfsq + s*hfsq, where hfsq = f*f/2.
   *      In order to guarantee error in log below 1ulp, we compute log
   *      by
   *              log(1+f) = f - s*(f - R)        (if f is not too large)
   *              log(1+f) = f - (hfsq - s*(hfsq+R)).     (better accuracy)
   *
   *      3. Finally,  log(x) = k*ln2 + log(1+f).
   *                          = k*ln2_hi+(f-(hfsq-(s*(hfsq+R)+k*ln2_lo)))
   *         Here ln2 is split into two floating point number:
   *                      ln2_hi + ln2_lo,
   *         where n*ln2_hi is always exact for |n| < 2000.
   */


#include <boost/math/special_functions/spherical_harmonic.hpp>


#include <Rcpp.h>
#include <boost/multi_array.hpp>

using namespace Rcpp;

// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(BH)]]

//'Estep in girt.
//'
//'@param x DataFrame.
//'@param a0 discrinimation parameter vector.
//'@param b0 difficulty parameter vector.
//'@param Xq node of theta dist.
//'@param AX weight of theta dist.
//'@param Yr node of phi dist.
//'@param BY weighit of phi dist.
//'@param D factor constant.
//'@param group a vector.
//'@param ind a design matrix for group.
//'@param resp a design matrix for person.
//'@param MLL a vector
//'@export
// [[Rcpp::export]]

List Estep_girt_mg(DataFrame x,
                NumericVector a0,
                NumericVector b0,
                NumericVector Xq,
                NumericMatrix AX, // multigroup
                NumericVector Yr,
                NumericMatrix BY, // multigroup
                double D,
                IntegerVector group,
                IntegerMatrix ind, // design matrix
                IntegerMatrix resp, // design matrix
                NumericVector MLL
){

  const int nj = x.length();// item n
  const int nn = x.nrows(); // subject n
  const int nq = Xq.length(); // node of theta
  const int nr = Yr.length(); // node of phi
  const int ng = max(group); // group n

  boost::multi_array <double, 4> knqr (boost::extents[ng][nn][nq][nr]);
  boost::multi_array <double, 4> hqr (boost::extents[ng][nn][nq][nr]);
  boost::multi_array <double, 4> Njqr (boost::extents[ng][nj][nq][nr]);
  boost::multi_array <double, 4> rjqr (boost::extents[ng][nj][nq][nr]);
  //Dimension d(nn,nq,nr);
  //NumericVector knqr(d);

  // もとのデータフレームから項目反応パタンだけを抜き出し，行列として保存
  NumericMatrix xall(nn, nj);
  for (int j=0; j<nj; j++){
    NumericVector kk = x[j];
    for(int i=0; i<nn; i++){
      double k = kk[i];
      xall(i,j) = k;
    }
  }

  int u;
  double phi, theta, a, b, tt, t, A, e, p;
  for(int g=0; g<ng; g++){
    for(int r=0; r<nr; r++){
      phi = Yr[r];
      //Rprintf("%d % / 100%\r", 100/nr*(r+1));
      for(int q=0; q<nq; q++){
        theta = Xq[q];
        for(int i=0; i<nn; i++){
          if(group[i] != g+1) continue; // 集団に属さない受験者の部分はスキップ
          t = 1.0;
          for(int j=0; j<nj; j++){
            a = a0[j];
            b = b0[j];
            A = sqrt(1+phi*phi*a*a);
            e = exp(-D*a/A*(theta-b));
            p = 1/(1+e);
            u = xall(i,j);
            if(u == 1){ // correct
              tt = p;
            } else if (u == 0) { // incorrect
              tt = 1.0-p;
            } else { // NA
              tt = 1;
            }
            t = t*tt;
          }
          knqr[g][i][q][r] = t*AX[q]*BY[r];
        }
      }
    }
  }

  // 各受検者のtheta, phiごとに事後分布の重みを計算する。
  // 規格化
  double f = 0; // 周辺対数尤度代入用
  for(int g=0; g<ng; g++){
    for(int i=0; i<nn; i++){
      if(group[i] != g+1) continue; // 集団に属さない受験者の部分はスキップ
      double Z = 0;
      for(int q=0; q<nq; q++){ // 総和を1にするための分母の計算 // sum
        for(int r=0; r<nr; r++){
          Z += knqr[g][i][q][r];
        }
      }
      f += log(Z);
      for(int q=0; q<nq; q++){ // 総和を1にするための分母の計算 // sum
        for(int r=0; r<nr; r++){
          double l = knqr[g][i][q][r];
          hqr[g][i][q][r] =  l / Z;
        }
      }
    }
  }
  MLL.push_back(f);
  if(traits::is_nan<REALSXP>(f)){
    // 対数尤度の計算に失敗したら，計算を中止する。
    stop("Can't calculate marginal log likelihood.");
  }

  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の期待度数
      if(ind(g,j) == 0) continue;
      for(int q=0; q<nq; q++){
        for(int r=0; r<nr; r++){
          double k = 0;
          for(int i=0; i<nn; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
            if(resp(i,j)==0) continue;
            k += hqr[g][i][q][r];
          }
          Njqr[g][j][q][r]= k;
        }
      }
    }
  }

  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の正答受検者の期待度数
      if(ind(g,j) == 0) continue;
      for(int q=0; q<nq; q++){
        for(int r=0; r<nr; r++){
          double h = 0;
          for(int i=0; i<nn; i++){ // sum
            //double d = resp(i,j);
            if(resp(i,j) == 0) continue;
            h += xall(i,j)*hqr[g][i][q][r];
          }
          rjqr[g][j][q][r] = h;
        }
      }
    }
  }

  return List::create(_["knqr"]=knqr, _["hqr"]=hqr, _["Njqr"]=Njqr, _["rjqr"]=rjqr, _["MLL"]=MLL);

}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include <boost/program_options.hpp>
#include <cstddef>
#include <string>

#include "DataStructures/ComplexModalVector.hpp"
#include "DataStructures/DataBox/DataBox.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/SpinWeighted.hpp"
#include "DataStructures/Variables.hpp"
#include "DataStructures/VariablesTag.hpp"
#include "Evolution/Systems/Cce/BoundaryData.hpp"
#include "Evolution/Systems/Cce/ReducedWorldtubeModeRecorder.hpp"
#include "Evolution/Systems/Cce/SpecBoundaryData.hpp"
#include "Evolution/Systems/Cce/Tags.hpp"
#include "Evolution/Systems/Cce/WorldtubeBufferUpdater.hpp"
#include "NumericalAlgorithms/Spectral/SwshCoefficients.hpp"
#include "NumericalAlgorithms/Spectral/SwshCollocation.hpp"
#include "Parallel/Printf.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

// Charm looks for this function but since we build without a main function or
// main module we just have it be empty
extern "C" void CkRegisterMainModule(void) {}

// from a time-varies-fastest set of buffers provided by
// `MetricWorldtubeH5BufferUpdater` extract the set of coefficients for a
// particular time given by `buffer_time_offset` into the `time_span` size of
// buffer.
void slice_buffers_to_libsharp_modes(
    const gsl::not_null<Variables<Cce::cce_metric_input_tags>*>
        coefficients_set,
    const Variables<Cce::cce_metric_input_tags>& coefficients_buffers,
    const size_t time_span, const size_t buffer_time_offset, const size_t l_max,
    const size_t computation_l_max) {
  SpinWeighted<ComplexModalVector, 0> spin_weighted_buffer;

  for (const auto& libsharp_mode :
       Spectral::Swsh::cached_coefficients_metadata(computation_l_max)) {
    for (size_t i = 0; i < 3; ++i) {
      for (size_t j = i; j < 3; ++j) {
        tmpl::for_each<
            tmpl::list<Cce::Tags::detail::SpatialMetric,
                       Cce::Tags::detail::Dr<Cce::Tags::detail::SpatialMetric>,
                       Tags::dt<Cce::Tags::detail::SpatialMetric>>>(
            [&i, &j, &libsharp_mode, &spin_weighted_buffer,
             &coefficients_buffers, &coefficients_set, &l_max,
             &computation_l_max, &time_span, &buffer_time_offset](auto tag_v) {
              using tag = typename decltype(tag_v)::type;
              spin_weighted_buffer.set_data_ref(
                  get<tag>(*coefficients_set).get(i, j).data(),
                  Spectral::Swsh::size_of_libsharp_coefficient_vector(
                      computation_l_max));
              if (libsharp_mode.l > l_max) {
                Spectral::Swsh::goldberg_modes_to_libsharp_modes_single_pair(
                    libsharp_mode, make_not_null(&spin_weighted_buffer), 0, 0.0,
                    0.0);

              } else {
                Spectral::Swsh::goldberg_modes_to_libsharp_modes_single_pair(
                    libsharp_mode, make_not_null(&spin_weighted_buffer), 0,
                    get<tag>(coefficients_buffers)
                        .get(i, j)[time_span *
                                       Spectral::Swsh::goldberg_mode_index(
                                           l_max, libsharp_mode.l,
                                           static_cast<int>(libsharp_mode.m)) +
                                   buffer_time_offset],
                    get<tag>(coefficients_buffers)
                        .get(i, j)[time_span *
                                       Spectral::Swsh::goldberg_mode_index(
                                           l_max, libsharp_mode.l,
                                           -static_cast<int>(libsharp_mode.m)) +
                                   buffer_time_offset]);
              }
            });
      }
      tmpl::for_each<tmpl::list<Cce::Tags::detail::Shift,
                                Cce::Tags::detail::Dr<Cce::Tags::detail::Shift>,
                                Tags::dt<Cce::Tags::detail::Shift>>>(
          [&i, &libsharp_mode, &spin_weighted_buffer, &coefficients_buffers,
           &coefficients_set, &l_max, &computation_l_max, &time_span,
           &buffer_time_offset](auto tag_v) {
            using tag = typename decltype(tag_v)::type;
            spin_weighted_buffer.set_data_ref(
                get<tag>(*coefficients_set).get(i).data(),
                Spectral::Swsh::size_of_libsharp_coefficient_vector(
                    computation_l_max));

            if (libsharp_mode.l > l_max) {
              Spectral::Swsh::goldberg_modes_to_libsharp_modes_single_pair(
                  libsharp_mode, make_not_null(&spin_weighted_buffer), 0, 0.0,
                  0.0);

            } else {
              Spectral::Swsh::goldberg_modes_to_libsharp_modes_single_pair(
                  libsharp_mode, make_not_null(&spin_weighted_buffer), 0,
                  get<tag>(coefficients_buffers)
                      .get(i)[time_span *
                                  Spectral::Swsh::goldberg_mode_index(
                                      l_max, libsharp_mode.l,
                                      static_cast<int>(libsharp_mode.m)) +
                              buffer_time_offset],
                  get<tag>(coefficients_buffers)
                      .get(i)[time_span *
                                  Spectral::Swsh::goldberg_mode_index(
                                      l_max, libsharp_mode.l,
                                      -static_cast<int>(libsharp_mode.m)) +
                              buffer_time_offset]);
            }
          });
    }
    tmpl::for_each<tmpl::list<Cce::Tags::detail::Lapse,
                              Cce::Tags::detail::Dr<Cce::Tags::detail::Lapse>,
                              Tags::dt<Cce::Tags::detail::Lapse>>>(
        [&libsharp_mode, &spin_weighted_buffer, &coefficients_buffers,
         &coefficients_set, &l_max, &computation_l_max, &time_span,
         &buffer_time_offset](auto tag_v) {
          using tag = typename decltype(tag_v)::type;
          spin_weighted_buffer.set_data_ref(
              get(get<tag>(*coefficients_set)).data(),
              Spectral::Swsh::size_of_libsharp_coefficient_vector(
                  computation_l_max));

          if (libsharp_mode.l > l_max) {
            Spectral::Swsh::goldberg_modes_to_libsharp_modes_single_pair(
                libsharp_mode, make_not_null(&spin_weighted_buffer), 0, 0.0,
                0.0);

          } else {
            Spectral::Swsh::goldberg_modes_to_libsharp_modes_single_pair(
                libsharp_mode, make_not_null(&spin_weighted_buffer), 0,
                get(get<tag>(coefficients_buffers))
                    [time_span * Spectral::Swsh::goldberg_mode_index(
                                     l_max, libsharp_mode.l,
                                     static_cast<int>(libsharp_mode.m)) +
                     buffer_time_offset],
                get(get<tag>(coefficients_buffers))
                    [time_span * Spectral::Swsh::goldberg_mode_index(
                                     l_max, libsharp_mode.l,
                                     -static_cast<int>(libsharp_mode.m)) +
                     buffer_time_offset]);
          }
        });
  }
}

// read in the data from a (previously standard) SpEC worldtube file
// `input_file`, perform the boundary computation, and dump the (considerably
// smaller) dataset associated with the spin-weighted scalars to `output_file`.
void perform_cce_worldtube_reduction(
    const std::string& input_file, const std::string& output_file,
    const size_t buffer_depth, const size_t l_max_factor,
    const bool fix_spec_normalization = false) {
  Cce::MetricWorldtubeH5BufferUpdater buffer_updater{input_file};
  const size_t l_max = buffer_updater.get_l_max();
  // Perform the boundary computation to scalars at twice the input l_max to be
  // absolutely certain that there are no problems associated with aliasing.
  const size_t computation_l_max = l_max_factor * l_max;

  // we're not interpolating, this is just a reasonable number of rows to ingest
  // at a time.
  const size_t size_of_buffer = square(l_max + 1) * (buffer_depth);
  const DataVector& time_buffer = buffer_updater.get_time_buffer();

  Variables<Cce::cce_metric_input_tags> coefficients_buffers{size_of_buffer};
  Variables<Cce::cce_metric_input_tags> coefficients_set{
      Spectral::Swsh::size_of_libsharp_coefficient_vector(computation_l_max)};

  Variables<Cce::Tags::characteristic_worldtube_boundary_tags<
      Cce::Tags::BoundaryValue>>
      boundary_data_variables{
          Spectral::Swsh::number_of_swsh_collocation_points(computation_l_max)};

  using reduced_boundary_tags =
      tmpl::list<Cce::Tags::BoundaryValue<Cce::Tags::BondiBeta>,
                 Cce::Tags::BoundaryValue<Cce::Tags::BondiU>,
                 Cce::Tags::BoundaryValue<Cce::Tags::BondiQ>,
                 Cce::Tags::BoundaryValue<Cce::Tags::BondiW>,
                 Cce::Tags::BoundaryValue<Cce::Tags::BondiJ>,
                 Cce::Tags::BoundaryValue<Cce::Tags::Dr<Cce::Tags::BondiJ>>,
                 Cce::Tags::BoundaryValue<Cce::Tags::Du<Cce::Tags::BondiJ>>,
                 Cce::Tags::BoundaryValue<Cce::Tags::BondiR>,
                 Cce::Tags::BoundaryValue<Cce::Tags::Du<Cce::Tags::BondiR>>>;

  size_t time_span_start = 0;
  size_t time_span_end = 0;
  Cce::ReducedWorldtubeModeRecorder recorder{output_file};

  ComplexModalVector output_goldberg_mode_buffer{square(computation_l_max + 1)};
  ComplexModalVector output_libsharp_mode_buffer{
      Spectral::Swsh::size_of_libsharp_coefficient_vector(computation_l_max)};

  for (size_t i = 0; i < time_buffer.size(); ++i) {
    const double time = time_buffer[i];
    Parallel::printf("reducing data at time : %f / %f \r", time,
                     time_buffer[time_buffer.size() - 1]);
    buffer_updater.update_buffers_for_time(
        make_not_null(&coefficients_buffers), make_not_null(&time_span_start),
        make_not_null(&time_span_end), time, l_max, 0, buffer_depth);

    slice_buffers_to_libsharp_modes(
        make_not_null(&coefficients_set), coefficients_buffers,
        time_span_end - time_span_start, i - time_span_start, l_max,
        computation_l_max);

    if (not buffer_updater.has_version_history() and fix_spec_normalization) {
      Cce::create_bondi_boundary_data_from_unnormalized_spec_modes(
          make_not_null(&boundary_data_variables),
          get<Cce::Tags::detail::SpatialMetric>(coefficients_set),
          get<Tags::dt<Cce::Tags::detail::SpatialMetric>>(coefficients_set),
          get<Cce::Tags::detail::Dr<Cce::Tags::detail::SpatialMetric>>(
              coefficients_set),
          get<Cce::Tags::detail::Shift>(coefficients_set),
          get<Tags::dt<Cce::Tags::detail::Shift>>(coefficients_set),
          get<Cce::Tags::detail::Dr<Cce::Tags::detail::Shift>>(
              coefficients_set),
          get<Cce::Tags::detail::Lapse>(coefficients_set),
          get<Tags::dt<Cce::Tags::detail::Lapse>>(coefficients_set),
          get<Cce::Tags::detail::Dr<Cce::Tags::detail::Lapse>>(
              coefficients_set),
          buffer_updater.get_extraction_radius(), computation_l_max);
    } else {
      Cce::create_bondi_boundary_data(
          make_not_null(&boundary_data_variables),
          get<Cce::Tags::detail::SpatialMetric>(coefficients_set),
          get<Tags::dt<Cce::Tags::detail::SpatialMetric>>(coefficients_set),
          get<Cce::Tags::detail::Dr<Cce::Tags::detail::SpatialMetric>>(
              coefficients_set),
          get<Cce::Tags::detail::Shift>(coefficients_set),
          get<Tags::dt<Cce::Tags::detail::Shift>>(coefficients_set),
          get<Cce::Tags::detail::Dr<Cce::Tags::detail::Shift>>(
              coefficients_set),
          get<Cce::Tags::detail::Lapse>(coefficients_set),
          get<Tags::dt<Cce::Tags::detail::Lapse>>(coefficients_set),
          get<Cce::Tags::detail::Dr<Cce::Tags::detail::Lapse>>(
              coefficients_set),
          buffer_updater.get_extraction_radius(), computation_l_max);
    }
    // loop over the tags that we want to dump.
    tmpl::for_each<reduced_boundary_tags>(
        [&recorder, &boundary_data_variables, &output_goldberg_mode_buffer,
         &output_libsharp_mode_buffer, &l_max, &computation_l_max,
         &time](auto tag_v) {
          using tag = typename decltype(tag_v)::type;
          SpinWeighted<ComplexModalVector, tag::type::type::spin>
              spin_weighted_libsharp_view;
          spin_weighted_libsharp_view.set_data_ref(
              output_libsharp_mode_buffer.data(),
              output_libsharp_mode_buffer.size());
          Spectral::Swsh::swsh_transform(
              computation_l_max, 1, make_not_null(&spin_weighted_libsharp_view),
              get(get<tag>(boundary_data_variables)));
          SpinWeighted<ComplexModalVector, tag::type::type::spin>
              spin_weighted_goldberg_view;
          spin_weighted_goldberg_view.set_data_ref(
              output_goldberg_mode_buffer.data(),
              output_goldberg_mode_buffer.size());
          Spectral::Swsh::libsharp_to_goldberg_modes(
              make_not_null(&spin_weighted_goldberg_view),
              spin_weighted_libsharp_view, computation_l_max);

          // The goldberg format type is in strictly increasing l modes, so to
          // reduce to a smaller l_max, we can just take the first (l_max + 1)^2
          // values.
          ComplexModalVector reduced_goldberg_view{
              output_goldberg_mode_buffer.data(), square(l_max + 1)};
          recorder.append_worldtube_mode_data(
              "/" + Cce::dataset_label_for_tag<tag>(), time,
              reduced_goldberg_view, l_max, tag::type::type::spin == 0);
        });
  }
  Parallel::printf("\n");
}

/*
 * This executable is used for converting the unnecessarily large SpEC worldtube
 * data format into a far smaller representation (roughly a factor of 4) just
 * storing the worldtube scalars that are required as input for CCE.
 */
int main(int argc, char** argv) {
  boost::program_options::positional_options_description pos_desc;
  pos_desc.add("old_spec_cce_file", 1).add("output_file", 1);

  boost::program_options::options_description desc("Options");
  desc.add_options()("help,h,", "show this help message")(
      "input_file", boost::program_options::value<std::string>()->required(),
      "name of old CCE data file")(
      "output_file", boost::program_options::value<std::string>()->required(),
      "output filename")(
      "fix_spec_normalization",
      "Apply corrections associated with documented SpEC "
      "worldtube file errors")(
      "buffer_depth",
      boost::program_options::value<size_t>()->default_value(2000),
      "number of time steps to load during each call to the file-accessing "
      "routines. Higher values mean fewer, larger loads from file into RAM.")(
      "lmax_factor", boost::program_options::value<size_t>()->default_value(2),
      "the boundary computations will be performed at a resolution that is "
      "lmax_factor times the input file lmax to avoid aliasing");

  boost::program_options::variables_map vars;

  boost::program_options::store(
      boost::program_options::command_line_parser(argc, argv)
          .positional(pos_desc)
          .options(desc)
          .run(),
      vars);

  if (vars.count("help") != 0u or vars.count("input_file") == 0u or
      vars.count("output_file") == 0u) {
    Parallel::printf("%s\n", desc);
    return 0;
  }

  perform_cce_worldtube_reduction(vars["input_file"].as<std::string>(),
                                  vars["output_file"].as<std::string>(),
                                  vars["buffer_depth"].as<size_t>(),
                                  vars["lmax_factor"].as<size_t>(),
                                  vars.count("fix_spec_normalization") != 0u);
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// 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_UNITS_SCALE_HPP_INCLUDED
#define BOOST_UNITS_SCALE_HPP_INCLUDED

#include <string>

#include <boost/units/config.hpp>
#include <boost/units/static_rational.hpp>
#include <boost/units/units_fwd.hpp>
#include <boost/units/detail/one.hpp>
#include <boost/units/detail/static_rational_power.hpp>

namespace boost {

namespace units {

template<class S, class Scale>
struct scaled_base_unit;

/// class representing a scaling factor such as 10^3
/// The exponent should be a static rational.
template<long Base, class Exponent>
struct scale
{
    static const long base = Base;
    typedef Exponent exponent;
    typedef double value_type;
    static value_type value() { return(detail::static_rational_power<Exponent>(static_cast<double>(base))); }
    static std::string name() { return ""; };
    static std::string symbol() { return ""; };
};

template<long Base, class Exponent>
const long scale<Base, Exponent>::base;

/// INTERNAL ONLY
template<long Base>
struct scale<Base, static_rational<0> >
{
    static const long base = Base;
    typedef static_rational<0> exponent;
    typedef one value_type;
    static one value() { one result; return(result); }
    static std::string name() { return(""); }
    static std::string symbol() { return(""); }
};

template<long Base>
const long scale<Base, static_rational<0> >::base;

template<long Base,class Exponent>
std::string symbol_string(const scale<Base,Exponent>&)
{
    return scale<Base,Exponent>::symbol();
}

template<long Base,class Exponent>
std::string name_string(const scale<Base,Exponent>&)
{
    return scale<Base,Exponent>::name();
}

#ifndef BOOST_UNITS_DOXYGEN

#define BOOST_UNITS_SCALE_SPECIALIZATION(base_,exponent_,val_,name_,symbol_) \
template<>                                                                   \
struct scale<base_, exponent_ >                                              \
{                                                                            \
    static const long base = base_;                                          \
    typedef exponent_ exponent;                                              \
    typedef double value_type;                                               \
    static value_type value()   { return(val_); }                            \
    static std::string name()   { return(#name_); }                          \
    static std::string symbol() { return(#symbol_); }                        \
}

#define BOOST_UNITS_SCALE_DEF(exponent_,value_,name_,symbol_)                 \
BOOST_UNITS_SCALE_SPECIALIZATION(10,static_rational<exponent_>,value_, name_, symbol_)

BOOST_UNITS_SCALE_DEF(-24, 1e-24 ,yocto, y);
BOOST_UNITS_SCALE_DEF(-21, 1e-21, zepto, z);
BOOST_UNITS_SCALE_DEF(-18, 1e-18, atto, a);
BOOST_UNITS_SCALE_DEF(-15, 1e-15, femto, f);
BOOST_UNITS_SCALE_DEF(-12, 1e-12, pico, p);
BOOST_UNITS_SCALE_DEF(-9, 1e-9, nano, n);
BOOST_UNITS_SCALE_DEF(-6, 1e-6, micro, u);
BOOST_UNITS_SCALE_DEF(-3, 1e-3, milli, m);
BOOST_UNITS_SCALE_DEF(-2, 1e-2, centi, c);
BOOST_UNITS_SCALE_DEF(-1, 1e-1, deci, d);

BOOST_UNITS_SCALE_DEF(1, 1e1, deka, da);
BOOST_UNITS_SCALE_DEF(2, 1e2, hecto, h);
BOOST_UNITS_SCALE_DEF(3, 1e3, kilo, k);
BOOST_UNITS_SCALE_DEF(6, 1e6, mega, M);
BOOST_UNITS_SCALE_DEF(9, 1e9, giga, G);
BOOST_UNITS_SCALE_DEF(12, 1e12, tera, T);
BOOST_UNITS_SCALE_DEF(15, 1e15, peta, P);
BOOST_UNITS_SCALE_DEF(18, 1e18, exa, E);
BOOST_UNITS_SCALE_DEF(21, 1e21, zetta, Z);
BOOST_UNITS_SCALE_DEF(24, 1e24, yotta, Y);

BOOST_UNITS_SCALE_SPECIALIZATION(2, static_rational<10>, 1024.0, kibi, Ki);
BOOST_UNITS_SCALE_SPECIALIZATION(2, static_rational<20>, 1048576.0, mebi, Mi);
BOOST_UNITS_SCALE_SPECIALIZATION(2, static_rational<30>, 1073741824.0, gibi, Gi);
BOOST_UNITS_SCALE_SPECIALIZATION(2, static_rational<40>, 1099511627776.0, tebi, Ti);
BOOST_UNITS_SCALE_SPECIALIZATION(2, static_rational<50>, 1125899906842624.0, pebi, Pi);
BOOST_UNITS_SCALE_SPECIALIZATION(2, static_rational<60>, 1152921504606846976.0, exbi, Ei);

#undef BOOST_UNITS_SCALE_DEF
#undef BOOST_UNITS_SCALE_SPECIALIZATION

#endif

} // namespace units

} // namespace boost

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::scale, (long)(class))

#endif

#endif


/*
 * BayesRRm.h
 *
 *  Created on: 5 Sep 2018
 *      Author: admin
 */

#ifndef SRC_BAYESRBASE_H_
#define SRC_BAYESRBASE_H_

#include "analysis.h"
#include "data.hpp"
#include "options.hpp"
#include "distributions_boost.hpp"
#include "colwriter.h"

#include <Eigen/Eigen>
#include <memory>
#include <shared_mutex>

class AnalysisGraph;

struct BayesRKernel;

class BayesRBase : public Analysis
{
public:
    explicit BayesRBase(const Data *data, const Options *opt);

    int runGibbs(AnalysisGraph* analysis) override; // where we run Gibbs sampling over the parametrised model

    void processColumn(const KernelPtr &kernel) override;

    std::unique_ptr<AsyncResult> processColumnAsync(const KernelPtr &kernel) override;
    void doThreadSafeUpdates(const ConstAsyncResultPtr& result) override;
    void updateGlobal(const KernelPtr& kernel, const ConstAsyncResultPtr &result) override;

    virtual void updateMu(double old_mu, double N)=0;

    void setDebugEnabled(bool enabled) { m_showDebug = enabled; }
    bool isDebugEnabled() const { return m_showDebug; }

protected:
    const string        m_outputFile;
    const string        m_iterLogFile; //debug file for iteration quantities
    const unsigned int  m_seed;
    const unsigned int  m_maxIterations;
    const unsigned int  m_burnIn;
    const unsigned int  m_thinning;
    const double        m_sigma0=0.0001;
    const double        m_v0E;
    const double        m_s02E;
    const double        m_v0G;
    const double        m_s02G;
    MatrixXd            m_cva;
    Distributions_boost m_dist;
    bool m_showDebug;

    bool m_colLog=false;       //log for columns
    ColWriter m_colWriter;//writer for log for columns
    string m_colLogFile;

  // Component variables
    MatrixXd m_priorPi;   // prior probabilities for each component
    MatrixXd m_pi;        // mixture probabilities
    VectorXd m_cVa;       // component-specific variance
    VectorXd m_muk;       // mean of k-th component marker effect size
    VectorXd m_denom;     // temporal variable for computing the inflation of the effect variance for a given non-zero componnet
    int m_m0;             // total number of markers in model
    MatrixXd m_v;         // variable storing the component assignment
    VectorXd m_cVaI;      // inverse of the component variances

    // Mean and residual variables
    double m_mu;          // mean or intercept
    VectorXd m_sigmaG;    // genetic variance
    double m_sigmaE;      // residuals variance

    // Linear model variables
    VectorXd m_beta;       // effect sizes
    VectorXd m_acum;       // acum values, posterior inclusion probabilities per locus
    VectorXd m_y_tilde;    // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd m_epsilon;    // variable containing the residuals
    VectorXd m_betasqnG;
    double m_epsilonSum=0.0;
    VectorXd m_y;
    VectorXd m_components;

    //fixed effects
    VectorXd m_gamma;     // fixed effects coefficients
    const double       s02F    = 1.0;
    double m_sigmaF;      // covariates variance if using ridge;

    static const std::size_t PIndex = 0;
    static const std::size_t RandomNormIndex = 1;
    constexpr static const std::size_t RandomNumberColumns = 2; // p, randomNorm
    std::vector<std::array<double, RandomNumberColumns>> m_randomNumbers;

    bool m_isAsync = false;

    mutable std::shared_mutex m_mutex;

    void setAsynchronous(bool async) { m_isAsync = async; }

    virtual void init(int K, unsigned int markerCount, unsigned int individualCount);

    virtual void prepareForAnylsis();

    virtual void prepare(BayesRKernel *kernel);
    virtual void readWithSharedLock(BayesRKernel *kernel);
    virtual void writeWithUniqueLock(BayesRKernel *kernel);

    void printDebugInfo() const;
};

#endif /* SRC_BAYESRBASE_H_ */


#include <ompl/base/ScopedState.h>
#include <ompl/base/Planner.h>
#include <ompl/base/spaces/DubinsAirplaneStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>

#include <boost/program_options.hpp>
#include <fstream>

namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace po = boost::program_options;

double interpolation = 100;
double solverRunTime = 10;

bool isStateValid(const ob::SpaceInformation *si, const ob::State *state)
{
    return si->satisfiesBounds(state);
}

void plan()
{
    ob::StateSpacePtr space = std::make_shared<ob::DubinsAirplaneStateSpace>(0.5, 0.25);

    ob::RealVectorBounds bounds(3);
    bounds.setLow(-3);
    bounds.setHigh(3);

    space->as<ob::SimpleSE3StateSpace>()->setBounds(bounds);

    og::SimpleSetup ss(space);

    ob::SpaceInformation *si = ss.getSpaceInformation().get();
    ss.setStateValidityChecker([si](const ob::State *state) { return isStateValid(si, state); });

    si->setMotionValidator(std::make_shared<ob::DubinsAirplaneMotionValidator>(si));

    ob::ScopedState<ob::SimpleSE3StateSpace> start(space);
    start->setXYZ(0.5, 0.0, -1.0);
    start->setYaw(0.0);

    ob::ScopedState<ob::SimpleSE3StateSpace> goal(space);
    goal->setXYZ(-0.5, 0.0, 1.0);
    goal->setYaw(0.0);
    ss.setStartAndGoalStates(start, goal);

    auto planner(std::make_shared<og::RRTstar>(ss.getSpaceInformation()));
    ss.setPlanner(planner);

    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
    ss.setup();

    ob::PlannerStatus solved = ss.solve(solverRunTime);

    if (solved)
    {
        ss.simplifySolution();
        std::cout << "Found solution:" << std::endl;
        og::PathGeometric path = ss.getSolutionPath();
        path.interpolate(interpolation);
        path.printAsMatrix(std::cout);

        std::ofstream pathfile;
        pathfile.open("/home/bendike/ompl_ws/data/paths/path.txt");
        path.printAsMatrix(pathfile);
        pathfile.close();
    }
    else
        std::cout << "No solution found" << std::endl;
}

bool legalNumber(char *str)
{
    while (*str != 0)
    {
        if (!isdigit(*str++))
        {
            return false;
        }
    }
    return true;
}

void parseArgs(int argc, char *argv[])
{
    for (int i = 1; i < argc; i++)
    {
        char *arg = argv[i];
        std::cout << arg << " ";

        if (strcmp(arg, "--time") == 0 || strcmp(arg, "-t") == 0)
        {
            if (legalNumber(argv[i + 1]))
            {
                solverRunTime = atol(argv[i + 1]);
            }
        }

        if (strcmp(arg, "--interpolation") == 0 || strcmp(arg, "-i") == 0)
        {
            if (legalNumber(argv[i + 1]))
            {
                interpolation = atol(argv[i + 1]);
            }
        }
    }
    std::cout << "\n";
}

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

    plan();
}


#include <boost/test/unit_test.hpp>

#include <fc/crypto/dh.hpp>
#include <fc/exception/exception.hpp>

BOOST_AUTO_TEST_SUITE(fc_crypto)

BOOST_AUTO_TEST_CASE(dh_test)
{
    fc::diffie_hellman alice;
    BOOST_CHECK( alice.generate_params( 128, 5 ) );
    BOOST_CHECK( alice.generate_pub_key() );

    fc::diffie_hellman bob;
    bob.p = alice.p;
    BOOST_CHECK( bob.validate() );
    BOOST_CHECK( bob.generate_pub_key() );

    fc::diffie_hellman charlie;
    BOOST_CHECK( !charlie.validate() );
    BOOST_CHECK( !charlie.generate_pub_key() );
    charlie.p = alice.p;
    BOOST_CHECK( charlie.validate() );
    BOOST_CHECK( charlie.generate_pub_key() );

    BOOST_CHECK( alice.compute_shared_key( bob.pub_key ) );
    BOOST_CHECK( bob.compute_shared_key( alice.pub_key ) );
    BOOST_CHECK_EQUAL( alice.shared_key.size(), bob.shared_key.size() );
    BOOST_CHECK( !memcmp( alice.shared_key.data(), bob.shared_key.data(), alice.shared_key.size() ) );
    std::vector<char> alice_bob = alice.shared_key;

    BOOST_CHECK( alice.compute_shared_key( charlie.pub_key ) );
    BOOST_CHECK( charlie.compute_shared_key( alice.pub_key ) );
    BOOST_CHECK_EQUAL( alice.shared_key.size(), charlie.shared_key.size() );
    BOOST_CHECK( !memcmp( alice.shared_key.data(), charlie.shared_key.data(), alice.shared_key.size() ) );
    std::vector<char> alice_charlie = alice.shared_key;

    BOOST_CHECK( charlie.compute_shared_key( bob.pub_key ) );
    BOOST_CHECK( bob.compute_shared_key( charlie.pub_key ) );
    BOOST_CHECK_EQUAL( charlie.shared_key.size(), bob.shared_key.size() );
    BOOST_CHECK( !memcmp( charlie.shared_key.data(), bob.shared_key.data(), bob.shared_key.size() ) );
    std::vector<char> bob_charlie = charlie.shared_key;

    BOOST_CHECK( alice_bob.size() != alice_charlie.size() ||  memcmp( alice_bob.data(), alice_charlie.data(), alice_bob.size() ) );

    BOOST_CHECK( alice_bob.size() != bob_charlie.size() || memcmp( alice_bob.data(), bob_charlie.data(), alice_bob.size() ) );

    BOOST_CHECK( alice_charlie.size() != bob_charlie.size() || memcmp( alice_charlie.data(), bob_charlie.data(), alice_charlie.size() ) );

    alice.p.clear(); alice.p.push_back(100); alice.p.push_back(2);
    BOOST_CHECK( !alice.validate() );
    alice.p = bob.p;
    alice.g = 1;
    BOOST_CHECK( !alice.validate() );
}

static const std::string P(          "\344\005\357H\277<\305\356\006*\312\326\265\347;\363" );
static const std::string ALICE_PUB(  "\015\343p\243\344d\3600\015/UL\340\277\243\203" );
static const std::string ALICE_PRIV( "oJ'\363\227\243\346\215/V\253\036i\250\370\233" );
static const std::string BOB_PUB(    "26x`3\017A\365\334\022\031\231\032Y\242\242" );
static const std::string BOB_PRIV(   "d\300\327v?jG4\340\037k\221\230z\372\203" );
static const std::string SHARED_KEY( "\303.\236g\25767!\006\365u\341;o\241" );
BOOST_AUTO_TEST_CASE(dh_size_mismatch_test)
{
   fc::diffie_hellman alice;
   alice.p.insert( alice.p.begin(), P.begin(), P.end() );
   alice.pub_key.insert( alice.pub_key.begin(), ALICE_PUB.begin(), ALICE_PUB.end() );
   alice.priv_key.insert( alice.priv_key.begin(), ALICE_PRIV.begin(), ALICE_PRIV.end() );
   alice.g = 5;
   BOOST_CHECK( alice.validate() );

   fc::diffie_hellman bob;
   bob.p = alice.p;
   bob.pub_key.insert( bob.pub_key.begin(), BOB_PUB.begin(), BOB_PUB.end() );
   bob.priv_key.insert( bob.priv_key.begin(), BOB_PRIV.begin(), BOB_PRIV.end() );
   bob.g = alice.g;
   BOOST_CHECK( bob.validate() );

   BOOST_CHECK( alice.compute_shared_key( bob.pub_key ) );
   BOOST_CHECK( bob.compute_shared_key( alice.pub_key ) );
   BOOST_CHECK_EQUAL( 15u, alice.shared_key.size() );
   BOOST_CHECK_EQUAL( 15u, bob.shared_key.size() );
   BOOST_CHECK( !memcmp( alice.shared_key.data(), bob.shared_key.data(), alice.shared_key.size() ) );

   BOOST_CHECK_EQUAL( SHARED_KEY, std::string( alice.shared_key.begin(), alice.shared_key.end() ) );
}

BOOST_AUTO_TEST_SUITE_END()


#include "SMesh_type.h"
#include "Scene_surface_mesh_item.h"
#include "Scene_polygon_soup_item.h"
#include "Kernel_type.h"
#include "Scene.h"

#include <CGAL/Three/Polyhedron_demo_io_plugin_interface.h>
#include <CGAL/Three/Three.h>

#include <CGAL/IO/polygon_soup_io.h>
#include <CGAL/boost/graph/io.h>
#include <CGAL/Polygon_mesh_processing/polygon_soup_to_polygon_mesh.h>

#include <QColor>
#include <QString>
#include <QStringList>
#include <QMainWindow>
#include <QInputDialog>

#include <boost/property_map/function_property_map.hpp>

#include <cstdint>
#include <fstream>
#include <iostream>
#include <limits>
#include <vector>

using namespace CGAL::Three;
class Polyhedron_demo_stl_plugin :
  public QObject,
  public Polyhedron_demo_io_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_io_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.IOPluginInterface/1.90" FILE "stl_io_plugin.json")

public:
  QString nameFilters() const;
  QString name() const { return "stl_plugin"; }
  bool canLoad(QFileInfo fileinfo) const;
  QList<Scene_item*> load(QFileInfo fileinfo, bool& ok, bool add_to_scene=true);

  bool canSave(const CGAL::Three::Scene_item*);
  bool save(QFileInfo fileinfo,QList<CGAL::Three::Scene_item*>&);
};

QString Polyhedron_demo_stl_plugin::nameFilters() const {
  return "STL files (*.stl)";
}

bool Polyhedron_demo_stl_plugin::canLoad(QFileInfo) const {
  return true;
}

QList<Scene_item*>
Polyhedron_demo_stl_plugin::
load(QFileInfo fileinfo, bool& ok, bool add_to_scene){

  // Open file
  std::ifstream in(fileinfo.filePath().toUtf8(), std::ios::in | std::ios::binary);
  if(!in) {
    std::cerr << "Error! Cannot open file " << (const char*)fileinfo.filePath().toUtf8() << std::endl;
    ok = false;
    return QList<Scene_item*>();
  }
  in.close();
  if(fileinfo.size() == 0)
  {
    CGAL::Three::Three::warning( tr("The file you are trying to load is empty."));
    Scene_surface_mesh_item* item = new Scene_surface_mesh_item();
    item->setName(fileinfo.completeBaseName());
    ok = true;
    if(add_to_scene)
      CGAL::Three::Three::scene()->addItem(item);
    return QList<Scene_item*>()<<item;
  }
  std::vector<EPICK::Point_3> points;
  std::vector<std::vector<int> > triangles;
  if (!CGAL::read_polygon_soup(fileinfo.filePath().toUtf8().toStdString(), points, triangles))
  {
    std::cerr << "Error: invalid STL file" << std::endl;
    ok = false;
    return QList<Scene_item*>();
  }

  try
  {
    // Try building a surface_mesh
    SMesh* SM = new SMesh();
    if (CGAL::Polygon_mesh_processing::is_polygon_soup_a_polygon_mesh(triangles))
    {
      //auto pmap = boost::make_function_property_map<std::array<double, 3>, Point_3>(
      //              [](const std::array<double, 3>& a) { return Point_3(a[0], a[1], a[2]); });

      CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(points, triangles, *SM);
    }

    if(!SM->is_valid() || SM->is_empty()){
      std::cerr << "Error: Invalid facegraph" << std::endl;
    }
    else{
      Scene_surface_mesh_item* item = new Scene_surface_mesh_item(SM);
      item->setName(fileinfo.completeBaseName());
      ok = true;
      if(add_to_scene)
        CGAL::Three::Three::scene()->addItem(item);
      return QList<Scene_item*>()<<item;
    }
  }
  catch(...){}

  Scene_polygon_soup_item* item = new Scene_polygon_soup_item();
  item->setName(fileinfo.completeBaseName());
  item->load(points, triangles);
  ok = true;
  if(add_to_scene)
    CGAL::Three::Three::scene()->addItem(item);
  return QList<Scene_item*>()<<item;
}

bool Polyhedron_demo_stl_plugin::canSave(const CGAL::Three::Scene_item* item)
{
  return qobject_cast<const Scene_surface_mesh_item*>(item);
}

bool Polyhedron_demo_stl_plugin::
save(QFileInfo fileinfo,QList<CGAL::Three::Scene_item*>& items)
{
  Scene_item* item = items.front();
  const Scene_surface_mesh_item* sm_item =
    qobject_cast<const Scene_surface_mesh_item*>(item);

  if(!sm_item)
    return false;

  QStringList list;
  list << tr("Binary");
  list << tr("Ascii");
  bool ok = false;
  QString choice
    = QInputDialog::getItem(nullptr, tr("Save STL file"), tr("Format"), list, 0, false, &ok);

  if (!ok)
    return false;

  std::ofstream out(fileinfo.filePath().toUtf8(), std::ios::out | std::ios::binary);
  if ( choice == tr("Binary") )
    CGAL::set_mode(out, CGAL::IO::BINARY);
  else
  {
    CGAL::set_mode(out, CGAL::IO::ASCII);
    out.precision (std::numeric_limits<double>::digits10 + 2);
  }

  if (sm_item)
  {
    CGAL::write_STL(out, *sm_item->face_graph());
    items.pop_front();
    return true;
  }
  return false;
}

#include "STL_io_plugin.moc"


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_PREV_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_PREV_HPP_INCLUDED
#include <nt2/sdk/constant/properties.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <nt2/include/functions/select.hpp>
#include <nt2/include/functions/fast_frexp.hpp>
#include <nt2/include/functions/fast_ldexp.hpp>
#include <nt2/include/functions/next.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::prev_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::prev_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> : meta::strip<A0>{};//

    NT2_FUNCTOR_CALL(1)
    {
      return a0-One<A0>();
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::prev_, tag::cpu_,
                       (A0)(X),
                       ((simd_<real_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::prev_(tag::simd_<tag::real_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> : meta::strip<A0>{};//

    NT2_FUNCTOR_CALL(1)
    {
      return -next(-a0);
//       typedef typename meta::as_integer<A0, signed>::type itype;
//       A0 m;
//       itype expon;
//       boost::fusion::tie(m, expon) = fast_frexp(a0);
//       expon =  seladd(iseq(m, Mhalf<A0>()), expon, Mone<itype>());
//       A0 diff =  fast_ldexp(Mone<A0>(), expon-Nbdigits<A0>());
//       diff = sel(iseqz(diff)||iseqz(a0),  Mindenormal<A0>(), diff);
//       return a0+diff;
    }
  };
} }

#endif
// modified by jt the 04/01/2011