/*    Copyright (c) 2010-2016, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      160413    M. Van den Broeck File created
 *
 *    References
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

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

#include <Eigen/Core>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/unifiedStateModelElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebraTypes.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{
namespace unit_tests
{

//! Test the functionality of the time conversion functions.
BOOST_AUTO_TEST_SUITE( test_USM_Element_Conversions )

//! Unit test for conversion Keplerian orbital elements to Unified State Model elements.
BOOST_AUTO_TEST_CASE( testConvertKeplerianToUnifiedStateModelElements )
{
    using namespace orbital_element_conversions;
    using namespace unit_conversions;
    using mathematical_constants::PI;

    // Setting fraction tolerance for correctness evaluation
    double tolerance = 1.0E-14;

    // Declare gravitational parameter of central body
    const double centralBodyGravitationalParameter = 1.32712440018e20; // [m^3/s^2]

    // Initializing default Keplerian orbit
    basic_mathematics::Vector6d keplerianElements = basic_mathematics::Vector6d::Zero( 6 );
    keplerianElements( semiMajorAxisIndex ) = 1.5e11;
    keplerianElements( eccentricityIndex ) = 0.1;
    keplerianElements( inclinationIndex ) = convertDegreesToRadians( 50.0 );
    keplerianElements( argumentOfPeriapsisIndex ) = convertDegreesToRadians( 350.0 );
    keplerianElements( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 15.0 );
    keplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 10.0 );
    // Unified State Model element vector declaration
    Eigen::VectorXd expectedUnifiedStateModelElements
            = Eigen::VectorXd::Zero( 7 );
    Eigen::VectorXd computedUnifiedStateModelElements
            = Eigen::VectorXd::Zero( 7 );

    // Case 1: Elliptical prograde orbit (default case).
    {
        // Default case, so no modification necessary.

        // Expected Unified State Model elements [m/s,m/s,m/s,-,-,-,-].
        // (Results obtained using Matlab code).
        expectedUnifiedStateModelElements( CHodographIndex ) = 29894.5892222602;
        expectedUnifiedStateModelElements( Rf1HodographIndex ) = -260.548512780222;
        expectedUnifiedStateModelElements( Rf2HodographIndex ) = 2978.08312848463;
        expectedUnifiedStateModelElements( epsilon1QuaternionIndex ) = -0.419002703925548;
        expectedUnifiedStateModelElements( epsilon2QuaternionIndex ) = -0.0551627524676706;
        expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) = -0.118296904421275;
        expectedUnifiedStateModelElements( etaQuaternionIndex ) = -0.898554198280556;

        // Compute Unified State Model elements.
        computedUnifiedStateModelElements =
                convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                               centralBodyGravitationalParameter );

        // Check if computed Unified State Model elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedUnifiedStateModelElements,
                                           computedUnifiedStateModelElements, tolerance );

    }

    // Case 2: Hyperbolic retrograde orbit.
    {
        // Modify Keplerian elements [m,-,rad,rad,rad,rad], i.e. overwrite them.
        keplerianElements( semiMajorAxisIndex ) = -1.5e11;
        keplerianElements( eccentricityIndex ) = 2.0;
        keplerianElements( inclinationIndex ) = convertDegreesToRadians( 170.0 );
        keplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 10.0 );

        // Set expected Unified State Model elements [m/s,m/s,m/s,-,-,-,-].
        // (Results obtained using Matlab code).
        expectedUnifiedStateModelElements( CHodographIndex ) = 17173.1340579794;
        expectedUnifiedStateModelElements( Rf1HodographIndex ) = -2993.47450825659;
        expectedUnifiedStateModelElements( Rf2HodographIndex ) = 34215.5701963558;
        expectedUnifiedStateModelElements( epsilon1QuaternionIndex ) = -0.987672114350896; //Minor error?
        expectedUnifiedStateModelElements( epsilon2QuaternionIndex ) = -0.130029500651719;
        expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) = -0.0113761072309622;
        expectedUnifiedStateModelElements( etaQuaternionIndex )= -0.0864101132863834;

        // Compute Unified State Model elements.
        computedUnifiedStateModelElements =
                convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                               centralBodyGravitationalParameter );

        // Check if computed Unified State Model elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedUnifiedStateModelElements,
                                           computedUnifiedStateModelElements, tolerance );

    }

    // Case 3: Parabolic retrograde orbit.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        keplerianElements( semiLatusRectumIndex ) = 1.5e11;
        keplerianElements( eccentricityIndex ) = 1.0;
        keplerianElements( inclinationIndex ) = convertDegreesToRadians( 170.0 );
        keplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 170.0 );

        // Set expected Unified State Model elements [m/s,m/s,m/s,-,-,-,-].
        // (Results obtained using Matlab code).
        expectedUnifiedStateModelElements( CHodographIndex ) = 29744.7407136119;
        expectedUnifiedStateModelElements( Rf1HodographIndex ) = -2592.42496973134;
        expectedUnifiedStateModelElements( Rf2HodographIndex ) = 29631.5529950138;
        expectedUnifiedStateModelElements( epsilon1QuaternionIndex ) = -0.299561523151596;
        expectedUnifiedStateModelElements( epsilon2QuaternionIndex ) = 0.95008776981561;
        expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) = -0.0870727897926938;
        expectedUnifiedStateModelElements( etaQuaternionIndex ) = -0.00380168010402369;

        // Compute Unified State Model elements.
        computedUnifiedStateModelElements =
                convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                               centralBodyGravitationalParameter );

        // Check if computed Unified State Model elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedUnifiedStateModelElements,
                                           computedUnifiedStateModelElements, tolerance );
    }

    // Case 4: Circular prograde orbit with non-zero argument of pericenter, test for error.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        keplerianElements( eccentricityIndex ) = 0.0;
            // Eccentricity is zero, while argument of pericenter is non-zero -> should give error
        keplerianElements( inclinationIndex ) = convertDegreesToRadians( 50.0 );

        // Declare variable indicating whether an exception has been thrown.
        bool isExceptionFound = false;

        // Try computing the Unified State Model elements and catch the expected runtime error.
        try
        {
            computedUnifiedStateModelElements =
                    convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                                 centralBodyGravitationalParameter );
        }
        catch( std::runtime_error )
        {
            isExceptionFound = true;
        }

        // Check if runtime error has occured
        BOOST_CHECK( isExceptionFound );
    }

    // Case 5: 0 inclination orbit, test for error because longitude of ascending node is non-zero
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        keplerianElements( eccentricityIndex ) = 0.1;
        keplerianElements( inclinationIndex ) = 0.0;

        // Declare variable indicating whether an exception has been thrown.
        bool isExceptionFound = false;

        // Try computing the Unified State Model elements and catch the expected runtime error.
        try
        {
            computedUnifiedStateModelElements =
                    convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                                 centralBodyGravitationalParameter );
        }
        catch( std::runtime_error )
        {
            isExceptionFound = true;
        }

        // Check if runtime error has occured
        BOOST_CHECK( isExceptionFound );
    }

    // Case 6: 180 inclination orbit.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        keplerianElements( inclinationIndex ) = PI; // = 180 deg

        // Set expected Unified State Model elements [m/s,m/s,m/s,-,-,-,-]. (Results were calculated by
        // hand).
        expectedUnifiedStateModelElements( CHodographIndex ) = 29894.5892222602;
        expectedUnifiedStateModelElements( Rf1HodographIndex ) = -260.548512780222;
        expectedUnifiedStateModelElements( Rf2HodographIndex ) = 2978.08312848463;
        expectedUnifiedStateModelElements( epsilon1QuaternionIndex ) = -0.300705799504273;
        expectedUnifiedStateModelElements( epsilon2QuaternionIndex ) = 0.953716950748227;
        expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) = -6.11740603377039e-17;
        expectedUnifiedStateModelElements( etaQuaternionIndex ) = -2.67091715588637e-18;

        // Compute Unified State Model elements.
        computedUnifiedStateModelElements =
                convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                               centralBodyGravitationalParameter );

        // Because two elements are near-zero, a close fraction/percentage check will fail.
        // Therefore, 1.0 is added to the elements to avoid this
        expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) =
                expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) + 1.0;
        expectedUnifiedStateModelElements( etaQuaternionIndex ) =
                expectedUnifiedStateModelElements( etaQuaternionIndex ) + 1.0;
        computedUnifiedStateModelElements( epsilon3QuaternionIndex ) =
                computedUnifiedStateModelElements( epsilon3QuaternionIndex ) + 1.0;
        computedUnifiedStateModelElements( etaQuaternionIndex ) =
                computedUnifiedStateModelElements( etaQuaternionIndex ) + 1.0;

        // Check if computed elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedUnifiedStateModelElements,
                                           computedUnifiedStateModelElements, tolerance );
    }

    // Case 7: 0 eccentricity and inclination orbit.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        keplerianElements( eccentricityIndex ) = 0.0;
        keplerianElements( inclinationIndex ) = 0.0;
        keplerianElements( longitudeOfAscendingNodeIndex ) = 0.0; // Default value because of zero inclination
        keplerianElements( argumentOfPeriapsisIndex ) = 0.0; // Default value because of zero eccentricity

        // Expected Unified State Model elements [m/s,m/s,m/s,-,-,-,-].
        // (Results obtained using code archive B. Rˆmgens (2011)).
        expectedUnifiedStateModelElements( CHodographIndex ) = 29744.7407136119;
        expectedUnifiedStateModelElements( Rf1HodographIndex ) = 0;
        expectedUnifiedStateModelElements( Rf2HodographIndex ) = 0;
        expectedUnifiedStateModelElements( epsilon1QuaternionIndex ) = 0;
        expectedUnifiedStateModelElements( epsilon2QuaternionIndex ) = 0;
        expectedUnifiedStateModelElements( epsilon3QuaternionIndex ) = 0.996194698091746;
        expectedUnifiedStateModelElements( etaQuaternionIndex ) = 0.0871557427476581;

        // Compute Unified State Model elements.
        computedUnifiedStateModelElements =
                convertKeplerianToUnifiedStateModelElements( keplerianElements,
                                                               centralBodyGravitationalParameter );

        // Check if computed elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedUnifiedStateModelElements,
                                           computedUnifiedStateModelElements, tolerance );
    }

    // Case 8: 200 degree inclination orbit, test for error.
    {
        keplerianElements( inclinationIndex ) = convertDegreesToRadians( 200.0 );
        bool isExceptionFound = false;

        // Try to convert Kepler to Unified State Model Elements
        try
        {
            computedUnifiedStateModelElements = convertKeplerianToUnifiedStateModelElements
                    ( keplerianElements, centralBodyGravitationalParameter );
        }
        // Catch the expected runtime error, and set the boolean flag to true.
        catch ( std::runtime_error )
        {
            isExceptionFound = true;
        }

        // Check value of flag.
        BOOST_CHECK( isExceptionFound );
    }
}

//! Unit test for the conversion of Unified State Model elements to Keplerian elements
BOOST_AUTO_TEST_CASE( testConvertUnifiedStateModelToKeplerianElements )
{
    /* Used procedure:
      Because the Kepler to Unified State Model elements are verified, a subsequent conversion back
      to Keplerian elements should yield the same outcome as the input Keplerian state. This
      principle is used for verification.
     */

    using namespace orbital_element_conversions;
    using namespace unit_conversions;
    using mathematical_constants::PI;

    // Setting fraction tolerance for correctness evaluation
    double tolerance = 1.0E-14;

    // Declare gravitational parameter of central body
    const double centralBodyGravitationalParameter = 1.32712440018e20; // [m^3/s^2]

    // Initializing default Keplerian orbit
    basic_mathematics::Vector6d expectedKeplerianElements = Eigen::VectorXd::Zero( 6 );
    expectedKeplerianElements( semiMajorAxisIndex ) = 1.5e11;
    expectedKeplerianElements( eccentricityIndex ) = 0.1;
    expectedKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 50.0 );
    expectedKeplerianElements( argumentOfPeriapsisIndex ) = convertDegreesToRadians( 350.0 );
    expectedKeplerianElements( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 15.0 );
    expectedKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 170.0 );

    // Declaring computed output vector.
    basic_mathematics::Vector6d computedKeplerianElements = Eigen::VectorXd::Zero( 6 );

    // Case 1: Elliptical prograde orbit (default case).
    {
        // Default case, so no modification necessary.

        // Convert to Unified State Model elements and back.
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
    }

    // Case 2: Hyperbolic retrograde orbit.
    {
        // Modify Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( semiMajorAxisIndex ) = -1.5e11;
        expectedKeplerianElements( eccentricityIndex ) = 2.0;
        expectedKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 160.0 );
        expectedKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 10.0 ); // 170 is above limit

        // Convert to Unified State Model elements and back.
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
    }

    // Case 3: Parabolic retrograde orbit.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( semiLatusRectumIndex ) = 3.5e11;
        expectedKeplerianElements( eccentricityIndex ) = 1.0;
        expectedKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 90.0 );

        // Convert to Unified State Model elements and back.
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
       }

    // Case 4: Circular prograde orbit.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( semiMajorAxisIndex ) = 3.5e11;
        expectedKeplerianElements( eccentricityIndex ) = 0.0;
        expectedKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 70.0 );
        expectedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0; // For e = 0, undefined.

        // Convert to Unified State Model elements and back.
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
    }

    // Case 5: 0 inclination orbit,
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( eccentricityIndex ) = 0.3;
        expectedKeplerianElements( inclinationIndex ) = 0.0;
        expectedKeplerianElements( longitudeOfAscendingNodeIndex ) = 0.0; // Set to zero as for
        // non-inclined orbit planes, this parameter is undefined

        // Convert to Unified State Model elements and back.
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
    }

    // Case 6: 180 inclination orbit, test for error.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( semiMajorAxisIndex ) = 1.5e15;
        expectedKeplerianElements( inclinationIndex ) = PI;
        expectedKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 240.0 );

        // Declare variable indicating whether an exception has been thrown.
        bool isExceptionFound = false;

        // Try convert to Unified State Model elements and back and catch the expected runtime error.
        try
        {
            computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                 centralBodyGravitationalParameter ),
                        centralBodyGravitationalParameter );
        }
        catch( std::runtime_error )
        {
            isExceptionFound = true;
        }

        // Check if runtime error has occured
        BOOST_CHECK( isExceptionFound );
    }

    // Case 7: 0 eccentricity and inclination orbit.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( eccentricityIndex ) = 0.0;
            // argument of pericenter was set to 0 in case 4, so no error.
        expectedKeplerianElements( inclinationIndex ) = 0.0;
            // longitude of ascending node was set to 0 in case 5, so no error.

        // Convert to Unified State Model elements and back
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
    }

    // Case 8: true anomaly exceeding 180 degrees.
    {
        // Set Keplerian elements [m,-,rad,rad,rad,rad].
        expectedKeplerianElements( semiMajorAxisIndex ) = 1.5e11;
        expectedKeplerianElements( eccentricityIndex ) = 0.1;
        expectedKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 50.0 );
        expectedKeplerianElements( argumentOfPeriapsisIndex ) = convertDegreesToRadians( 350.0 );
        expectedKeplerianElements( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 15.0 );
        expectedKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 240.0 );

        // Convert to Unified State Model elements and back
        computedKeplerianElements = convertUnifiedStateModelToKeplerianElements(
                    convertKeplerianToUnifiedStateModelElements( expectedKeplerianElements,
                                                                   centralBodyGravitationalParameter ),
                    centralBodyGravitationalParameter );

        // Check if computed Keplerian elements match the expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerianElements,
                                           computedKeplerianElements, tolerance );
    }
}
BOOST_AUTO_TEST_SUITE_END( )

} // end namespace unit_tests
} // end namespace tudat


#include <boost/test/unit_test.hpp>
#include <sudoku/groups.h>
#include "util.h"

BOOST_AUTO_TEST_CASE(computeRowGroups_0x0)
{
    auto rowGroups = sudoku::computeRowGroups(0, 0);
    BOOST_REQUIRE(rowGroups.empty());
}

BOOST_AUTO_TEST_CASE(computeRowGroups_0x5)
{
    auto rowGroups = sudoku::computeRowGroups(0, 5);
    BOOST_REQUIRE(rowGroups.empty());
}

BOOST_AUTO_TEST_CASE(computeRowGroups_5x0)
{
    auto rowGroups = sudoku::computeRowGroups(5, 0);
    BOOST_REQUIRE(rowGroups.empty());
}

BOOST_AUTO_TEST_CASE(computeRowGroups_1x1)
{
    auto rowGroups = sudoku::computeRowGroups(1, 1);
    BOOST_REQUIRE_EQUAL(rowGroups.size(), 1);
    BOOST_REQUIRE_EQUAL_VECTORS(rowGroups[0], {0});
}

BOOST_AUTO_TEST_CASE(computeRowGroups_2x2)
{
    auto rowGroups = sudoku::computeRowGroups(2, 2);
    BOOST_REQUIRE_EQUAL(rowGroups.size(), 2);
    BOOST_REQUIRE_EQUAL_VECTORS(rowGroups[0], {0, 1});
    BOOST_REQUIRE_EQUAL_VECTORS(rowGroups[1], {2, 3});
}

BOOST_AUTO_TEST_CASE(computeRowGroups_3x4)
{
    auto rowGroups = sudoku::computeRowGroups(3, 4);
    BOOST_REQUIRE_EQUAL(rowGroups.size(), 3);
    BOOST_REQUIRE_EQUAL_VECTORS(rowGroups[0], {0, 1, 2, 3});
    BOOST_REQUIRE_EQUAL_VECTORS(rowGroups[1], {4, 5, 6, 7});
    BOOST_REQUIRE_EQUAL_VECTORS(rowGroups[2], {8, 9, 10, 11});
}

BOOST_AUTO_TEST_CASE(computeColumnGroups_0x0)
{
    auto columnGroups = sudoku::computeColumnGroups(0, 0);
    BOOST_REQUIRE(columnGroups.empty());
}

BOOST_AUTO_TEST_CASE(computeColumnGroups_0x5)
{
    auto columnGroups = sudoku::computeColumnGroups(0, 5);
    BOOST_REQUIRE(columnGroups.empty());
}

BOOST_AUTO_TEST_CASE(computeColumnGroups_5x0)
{
    auto columnGroups = sudoku::computeColumnGroups(5, 0);
    BOOST_REQUIRE(columnGroups.empty());
}

BOOST_AUTO_TEST_CASE(computeColumnGroups_1x1)
{
    auto columnGroups = sudoku::computeColumnGroups(1, 1);
    BOOST_REQUIRE_EQUAL(columnGroups.size(), 1);
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[0], {0});
}

BOOST_AUTO_TEST_CASE(computeColumnGroups_2x2)
{
    auto columnGroups = sudoku::computeColumnGroups(2, 2);
    BOOST_REQUIRE_EQUAL(columnGroups.size(), 2);
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[0], {0, 2});
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[1], {1, 3});
}

BOOST_AUTO_TEST_CASE(computeColumnGroups_3x4)
{
    auto columnGroups = sudoku::computeColumnGroups(3, 4);
    BOOST_REQUIRE_EQUAL(columnGroups.size(), 4);
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[0], {0, 4, 8});
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[1], {1, 5, 9});
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[2], {2, 6, 10});
    BOOST_REQUIRE_EQUAL_VECTORS(columnGroups[3], {3, 7, 11});
}

BOOST_AUTO_TEST_CASE(computeSquareGroups_9x9)
{
    auto squareGroups = sudoku::computeSquareGroups(3);
    BOOST_REQUIRE_EQUAL(squareGroups.size(), 9);
    BOOST_REQUIRE_EQUAL_VECTORS(squareGroups[0], {0, 1, 2, 9, 10, 11, 18, 19, 20});
    BOOST_REQUIRE_EQUAL_VECTORS(squareGroups[5], {33, 34, 35, 42, 43, 44, 51, 52, 53});
}

BOOST_AUTO_TEST_CASE(computeGroupsFromMap_emptyString)
{
    auto groups = sudoku::computeGroupsFromMap("");
    BOOST_REQUIRE_EQUAL(groups.size(), 0);
}

BOOST_AUTO_TEST_CASE(computeGroupsFromMap_simple)
{
    auto groups = sudoku::computeGroupsFromMap(
        "0 0 1 1 1 2 "
        "0 0 1 1 1 2 "
        "0 0 0 0 1 2 "
        "0 0 1 1 1 2 "
    );
    BOOST_REQUIRE_EQUAL(groups.size(), 3);
    BOOST_REQUIRE_EQUAL_VECTORS(groups[0], {0, 1, 6, 7, 12, 13, 14, 15, 18, 19});
    BOOST_REQUIRE_EQUAL_VECTORS(groups[1], {2, 3, 4, 8, 9, 10, 16, 20, 21, 22});
    BOOST_REQUIRE_EQUAL_VECTORS(groups[2], {5, 11, 17, 23});
}

BOOST_AUTO_TEST_CASE(joinGroups_empty)
{
    auto groups = sudoku::joinGroups({});
    BOOST_REQUIRE(groups.empty());
}

BOOST_AUTO_TEST_CASE(joinGroups_emptyVectors)
{
    auto groups = sudoku::joinGroups({ {}, {}, {} });
    BOOST_REQUIRE(groups.empty());
}

BOOST_AUTO_TEST_CASE(joinGroups_simple)
{
    std::vector<std::vector<sudoku::CellCount>> groups1 = {  // 2 groups
        {0, 1, 2, 3},
        {4, 5, 6, 7}
    };
    std::vector<std::vector<sudoku::CellCount>> groups2 = {  // 4 groups
        {0, 4},
        {1, 5},
        {2, 6},
        {3, 7}
    };
    auto allGroups = sudoku::joinGroups({groups1, groups2});  // 2 + 4 = 6 groups
    BOOST_REQUIRE_EQUAL(allGroups.size(), 6);
    BOOST_REQUIRE_EQUAL_VECTORS(allGroups[0], {0, 1, 2, 3});
    BOOST_REQUIRE_EQUAL_VECTORS(allGroups[1], {4, 5, 6, 7});
    BOOST_REQUIRE_EQUAL_VECTORS(allGroups[2], {0, 4});
    BOOST_REQUIRE_EQUAL_VECTORS(allGroups[3], {1, 5});
    BOOST_REQUIRE_EQUAL_VECTORS(allGroups[4], {2, 6});
    BOOST_REQUIRE_EQUAL_VECTORS(allGroups[5], {3, 7});
}


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <functional>
#include <Eigen/Dense>
#include <boost/math/special_functions/fpclassify.hpp> // isnan

namespace vinecopulib {

//! Tools for working with Eigen types
namespace tools_eigen {
//! An `Eigen::Matrix` containing `bool`s (similar to `Eigen::MatrixXd`).
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> MatrixXb;

template<typename T>
Eigen::MatrixXd unaryExpr_or_nan(const Eigen::MatrixXd &x, const T &func)
{
    return x.unaryExpr([&func](const double &y) {
        if ((boost::math::isnan)(y)) {
            return std::numeric_limits<double>::quiet_NaN();
        } else {
            return func(y);
        }
    });
}

template<typename T>
Eigen::VectorXd binaryExpr_or_nan(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u,
    const T &func)
{
    auto func_or_nan = [&func](const double &u1, const double &u2) {
        if ((boost::math::isnan)(u1) | (boost::math::isnan)(u2)) {
            return std::numeric_limits<double>::quiet_NaN();
        } else {
            return func(u1, u2);
        }
    };
    return u.col(0).binaryExpr(u.col(1), func_or_nan);
}

void remove_nans(Eigen::MatrixXd &x);

void remove_nans(Eigen::MatrixXd &x, Eigen::VectorXd &weights);

bool check_if_in_unit_cube(const Eigen::MatrixXd &u);

Eigen::Matrix<double, Eigen::Dynamic, 2> swap_cols(
    Eigen::Matrix<double, Eigen::Dynamic, 2> u);

Eigen::VectorXd invert_f(
    const Eigen::VectorXd &x,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f,
    const double lb = 1e-20,
    const double ub = 1 - 1e-20,
    int n_iter = 35);

Eigen::Matrix<double, Eigen::Dynamic, 2> expand_grid(
    const Eigen::VectorXd &grid_points);

Eigen::MatrixXd read_matxd(const char *filename,
                           int max_buffer_size = static_cast<int>(1e6));

Eigen::Matrix <size_t, Eigen::Dynamic, Eigen::Dynamic> read_matxs(
    const char *filename, int max_buffer_size = static_cast<int>(1e6));

}

}

#include <vinecopulib/misc/implementation/tools_eigen.ipp>


//
// Created by a.kiryanenko on 2020-02-04.
//

#include <boost/test/unit_test.hpp>
#include <libspu.hpp>
#include <structure.hpp>

BOOST_AUTO_TEST_SUITE(testSpuApi)

    using namespace SPU;
    using namespace boost;

    struct Fixture
    {
        Structure<> structure;

        Fixture() : structure() {}
        ~Fixture() = default;
    };

    BOOST_AUTO_TEST_CASE(test_mask)
    {
        auto mask = FieldsLength<int>::mask(4);
        BOOST_CHECK_EQUAL(mask[0], 0xF);
        BOOST_CHECK_EQUAL(mask[1], 0);

        mask = FieldsLength<int>::mask(32);
        BOOST_CHECK_EQUAL(mask[0], 0xFFFFFFFF);
        BOOST_CHECK_EQUAL(mask[1], 0);

        mask = FieldsLength<int>::mask(36);
        BOOST_CHECK_EQUAL(mask[0], 0xFFFFFFFF);
        BOOST_CHECK_EQUAL(mask[1], 0xF);

        mask = FieldsLength<int>::mask(64);
        BOOST_CHECK_EQUAL(mask[0], 0xFFFFFFFF);
        BOOST_CHECK_EQUAL(mask[1], 0xFFFFFFFF);
    }

    BOOST_FIXTURE_TEST_CASE(test_nsm_command, Fixture) {
        auto resp = structure.nsm(123);
        BOOST_CHECK_EQUAL(resp.status, ERR);

        structure.insert(5, 55);
        resp = structure.nsm(123);
        BOOST_CHECK_EQUAL(resp.status, OK);
        BOOST_CHECK_EQUAL((int) resp.key, 5);
        BOOST_CHECK_EQUAL((int) resp.value, 55);

        resp = structure.nsm(3);
        BOOST_CHECK_EQUAL(resp.status, ERR);
    }

    BOOST_FIXTURE_TEST_CASE(test_prev_command, Fixture) {
        auto resp = structure.prev(123);
        BOOST_CHECK_EQUAL(resp.status, ERR);

        structure.insert(10, 10);
        resp = structure.prev(10);
        BOOST_CHECK_EQUAL(resp.status, ERR);

        structure.insert(5, 55);
        resp = structure.prev(10);
        BOOST_CHECK_EQUAL(resp.status, OK);
        BOOST_CHECK_EQUAL((int) resp.key, 5);
        BOOST_CHECK_EQUAL((int) resp.value, 55);

        resp = structure.prev(123);
        BOOST_CHECK_EQUAL(resp.status, ERR);

        resp = structure.prev(3);
        BOOST_CHECK_EQUAL(resp.status, ERR);
    }

BOOST_AUTO_TEST_SUITE_END()


#include "teca_config.h"
#include "teca_cf_reader.h"
#include "teca_multi_cf_reader.h"
#include "teca_normalize_coordinates.h"
#include "teca_l2_norm.h"
#include "teca_vorticity.h"
#include "teca_derived_quantity.h"
#include "teca_derived_quantity_numerics.h"
#include "teca_tc_candidates.h"
#include "teca_tc_trajectory.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_table_reader.h"
#include "teca_table_reduce.h"
#include "teca_table_sort.h"
#include "teca_table_calendar.h"
#include "teca_table_writer.h"
#include "teca_mpi_manager.h"
#include "teca_coordinate_util.h"
#include "teca_app_util.h"
#include "teca_calcalcs.h"

#include <vector>
#include <string>
#include <iostream>
#include <boost/program_options.hpp>

using namespace std;
using namespace teca_derived_quantity_numerics;

using boost::program_options::value;

// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // initialize mpi
    teca_mpi_manager mpi_man(argc, argv);

    // initialize command line options description
    // set up some common options to simplify use for most
    // common scenarios
    int help_width = 100;
    options_description basic_opt_defs(
        "Basic usage:\n\n"
        "The following options are the most commonly used. Information\n"
        "on advanced options can be displayed using --advanced_help\n\n"
        "Basic command line options", help_width, help_width - 4
        );
    basic_opt_defs.add_options()
        ("input_file", value<std::string>(), "\na teca_multi_cf_reader configuration file"
            " identifying the set of NetCDF CF2 files to process. When present data is"
            " read using the teca_multi_cf_reader. Use one of either --input_file or"
            " --input_regex.\n")

        ("input_regex", value<std::string>(), "\na teca_cf_reader regex identifying the"
            " set of NetCDF CF2 files to process. When present data is read using the"
            " teca_cf_reader. Use one of either --input_file or --input_regex.\n")

        ("candidate_file", value<string>()->default_value("candidates.bin"),
            "\nfile path to write the storm candidates to. The extension determines"
            " the file format. May be one of `.nc`, `.csv`, or `.bin`\n")

        ("850mb_wind_u", value<string>()->default_value("U850"), "\nname of variable with 850 mb wind x-component\n")
        ("850mb_wind_v", value<string>()->default_value("V850"), "\nname of variable with 850 mb wind x-component\n")
        ("surface_wind_u", value<string>()->default_value("UBOT"), "\nname of variable with surface wind x-component\n")
        ("surface_wind_v", value<string>()->default_value("VBOT"), "\nname of variable with surface wind y-component\n")
        ("sea_level_pressure", value<string>()->default_value("PSL"), "\nname of variable with sea level pressure\n")
        ("500mb_temp", value<string>()->default_value("T500"), "\nname of variable with 500mb temperature for warm core calc\n")
        ("200mb_temp", value<string>()->default_value("T200"), "\nname of variable with 200mb temperature for warm core calc\n")
        ("1000mb_height", value<string>()->default_value("Z1000"), "\nname of variable with 1000mb height for thickness calc\n")
        ("200mb_height", value<string>()->default_value("Z200"), "\nname of variable with 200mb height for thickness calc\n")
        ("storm_core_radius", value<double>()->default_value(2.0),
            "\nmaximum number of degrees latitude separationi between vorticity max and pressure min defining a storm\n")
        ("min_vorticity", value<double>()->default_value(1.6e-4, "1.6e-4"), "\nminimum vorticty to be considered a tropical storm\n")
        ("vorticity_window", value<double>()->default_value(7.74446, "7.74446"),
            "\nsize of the search window in degrees. storms core must have a local vorticity max centered on this window\n")
        ("pressure_delta", value<double>()->default_value(400.0), "\nmaximum pressure change within specified radius\n")
        ("pressure_delta_radius", value<double>()->default_value(5.0),
            "\nradius in degrees over which max pressure change is computed\n")
        ("core_temp_delta", value<double>()->default_value(0.8, "0.8"), "\nmaximum core temperature change over the specified radius\n")
        ("core_temp_radius", value<double>()->default_value(5.0), "\nradius in degrees over which max core temperature change is computed\n")
        ("thickness_delta", value<double>()->default_value(50.0), "\nmaximum thickness change over the specified radius\n")
        ("thickness_radius", value<double>()->default_value(4.0), "\nradius in degrees over with max thickness change is computed\n")
        ("lowest_lat", value<double>()->default_value(80), "\nlowest latitude in degrees to search for storms\n")
        ("highest_lat", value<double>()->default_value(80), "\nhighest latitude in degrees to search for storms\n")
        ("max_daily_distance", value<double>()->default_value(1600), "\nmax distance in km that a storm can travel in one day\n")
        ("min_wind_speed", value<double>()->default_value(17.0), "\nminimum peak wind speed to be considered a tropical storm\n")
        ("min_wind_duration", value<double>()->default_value(2.0), "\nnumber of, not necessarily consecutive, days min wind speed sustained\n")

        ("track_file", value<string>()->default_value("tracks.bin"), "\nfile path to"
            " write storm tracks to. The extension determines the file format. May be"
            " one of `.nc`, `.csv`, or `.bin`\n")

        ("first_step", value<long>()->default_value(0), "\nfirst time step to process\n")
        ("last_step", value<long>()->default_value(-1), "\nlast time step to process\n")
        ("start_date", value<std::string>(), "\nThe first time to process in 'Y-M-D h:m:s'"
            " format. Note: There must be a space between the date and time specification\n")
        ("end_date", value<std::string>(), "\nThe last time to process in 'Y-M-D h:m:s' format\n")
        ("n_threads", value<int>()->default_value(-1), "\nSets the thread pool size on each"
            " MPI rank. When the default value of -1 is used TECA will coordinate the thread"
            " pools across ranks such each thread is bound to a unique physical core.\n")

        ("help", "\ndisplays documentation for application specific command line options\n")
        ("advanced_help", "\ndisplays documentation for algorithm specific command line options\n")
        ("full_help", "\ndisplays both basic and advanced documentation together\n")
        ;

    // add all options from each pipeline stage for more advanced use
    options_description advanced_opt_defs(
        "Advanced usage:\n\n"
        "The following list contains the full set options giving one full\n"
        "control over all runtime modifiable parameters. The basic options\n"
        "(see" "--help) map to these, and will override them if both are\n"
        "specified.\n\n"
        "tropical storms pipeline:\n\n"
        "   (cf / mcf_reader)\n"
        "         \\\n"
        "  (surface_wind_speed)--(850mb_vorticity)--(core_temperature)\n"
        "                                                    /\n"
        " (tracks)--(sort)--(map_reduce)--(candidates)--(thickness)\n"
        "     \\                               /\n"
        " (track_writer)              (candidate_writer)\n\n"
        "Advanced command line options", help_width, help_width - 4
        );

    // create the pipeline stages here, they contain the
    // documentation and parse command line.
    // objects report all of their properties directly
    // set default options here so that command line options override
    // them. while we are at it connect the pipeline
    p_teca_cf_reader cf_reader = teca_cf_reader::New();
    cf_reader->get_properties_description("cf_reader", advanced_opt_defs);

    p_teca_multi_cf_reader mcf_reader = teca_multi_cf_reader::New();
    mcf_reader->get_properties_description("mcf_reader", advanced_opt_defs);

    p_teca_normalize_coordinates sim_coords = teca_normalize_coordinates::New();

    p_teca_l2_norm surf_wind = teca_l2_norm::New();
    surf_wind->set_component_0_variable("UBOT");
    surf_wind->set_component_1_variable("VBOT");
    surf_wind->set_l2_norm_variable("surface_wind");
    surf_wind->get_properties_description("surface_wind_speed", advanced_opt_defs);

    p_teca_vorticity vort_850mb = teca_vorticity::New();
    vort_850mb->set_component_0_variable("U850");
    vort_850mb->set_component_1_variable("V850");
    vort_850mb->set_vorticity_variable("850mb_vorticity");
    vort_850mb->get_properties_description("850mb_vorticity", advanced_opt_defs);

    p_teca_derived_quantity core_temp = teca_derived_quantity::New();
    core_temp->set_dependent_variables({"T500", "T200"});
    core_temp->set_derived_variable("core_temperature");
    core_temp->get_properties_description("core_temperature", advanced_opt_defs);

    p_teca_derived_quantity thickness = teca_derived_quantity::New();
    thickness->set_dependent_variables({"Z1000", "Z200"});
    thickness->set_derived_variable("thickness");
    thickness->get_properties_description("thickness", advanced_opt_defs);

    p_teca_tc_candidates candidates = teca_tc_candidates::New();
    candidates->set_surface_wind_speed_variable("surface_wind");
    candidates->set_vorticity_850mb_variable("850mb_vorticity");
    candidates->set_sea_level_pressure_variable("PSL");
    candidates->set_core_temperature_variable("core_temperature");
    candidates->set_thickness_variable("thickness");
    candidates->set_max_core_radius(2.0);
    candidates->set_min_vorticity_850mb(1.6e-4);
    candidates->set_vorticity_850mb_window(7.74446);
    candidates->set_max_pressure_delta(400.0);
    candidates->set_max_pressure_radius(5.0);
    candidates->set_max_core_temperature_delta(0.8);
    candidates->set_max_core_temperature_radius(5.0);
    candidates->set_max_thickness_delta(50.0);
    candidates->set_max_thickness_radius(4.0);
    candidates->set_search_lat_low(-80.0);
    candidates->set_search_lat_high(80.0);
    candidates->get_properties_description("candidates", advanced_opt_defs);

    p_teca_table_reduce map_reduce = teca_table_reduce::New();
    map_reduce->get_properties_description("map_reduce", advanced_opt_defs);

    p_teca_table_writer candidate_writer = teca_table_writer::New();
    candidate_writer->set_output_format_auto();
    candidate_writer->get_properties_description("candidate_writer", advanced_opt_defs);

    p_teca_table_sort sort = teca_table_sort::New();
    sort->set_index_column("storm_id");
    sort->get_properties_description("sort", advanced_opt_defs);

    p_teca_tc_trajectory tracks = teca_tc_trajectory::New();
    tracks->set_max_daily_distance(1600.0);
    tracks->set_min_wind_speed(17.0);
    tracks->set_min_wind_duration(2.0);
    tracks->get_properties_description("tracks", advanced_opt_defs);

    p_teca_table_calendar calendar = teca_table_calendar::New();
    calendar->get_properties_description("calendar", advanced_opt_defs);

    p_teca_table_writer track_writer = teca_table_writer::New();
    track_writer->set_output_format_auto();
    track_writer->get_properties_description("track_writer", advanced_opt_defs);

    // package basic and advanced options for display
    options_description all_opt_defs(help_width, help_width - 4);
    all_opt_defs.add(basic_opt_defs).add(advanced_opt_defs);

    // parse the command line
    int ierr = 0;
    variables_map opt_vals;
    if ((ierr = teca_app_util::process_command_line_help(
        mpi_man.get_comm_rank(), argc, argv, basic_opt_defs,
        advanced_opt_defs, all_opt_defs, opt_vals)))
    {
        if (ierr == 1)
            return 0;
        return -1;
    }

    // pass command line arguments into the pipeline objects
    // advanced options are processed first, so that the basic
    // options will override them
    cf_reader->set_properties("cf_reader", opt_vals);
    mcf_reader->set_properties("mcf_reader", opt_vals);
    surf_wind->set_properties("surface_wind_speed", opt_vals);
    vort_850mb->set_properties("850mb_vorticity", opt_vals);
    core_temp->set_properties("core_temperature", opt_vals);
    thickness->set_properties("thickness", opt_vals);
    candidates->set_properties("candidates", opt_vals);
    map_reduce->set_properties("map_reduce", opt_vals);
    candidate_writer->set_properties("candidate_writer", opt_vals);
    sort->set_properties("sort", opt_vals);
    tracks->set_properties("tracks", opt_vals);
    calendar->set_properties("calendar", opt_vals);
    track_writer->set_properties("track_writer", opt_vals);

    // now pass in the basic options, these are processed
    // last so that they will take precedence

    // configure the reader
    bool have_file = opt_vals.count("input_file");
    bool have_regex = opt_vals.count("input_regex");

    p_teca_algorithm reader;
    if (opt_vals.count("input_file"))
    {
        mcf_reader->set_input_file(opt_vals["input_file"].as<string>());
        reader = mcf_reader;
    }
    else if (opt_vals.count("input_regex"))
    {
        cf_reader->set_files_regex(opt_vals["input_regex"].as<string>());
        reader = cf_reader;
    }

    if (!opt_vals["850mb_wind_u"].defaulted())
        vort_850mb->set_component_0_variable(
            opt_vals["850mb_wind_u"].as<string>());

    if (!opt_vals["850mb_wind_v"].defaulted())
        vort_850mb->set_component_1_variable(
            opt_vals["850mb_wind_v"].as<string>());

    if (!opt_vals["surface_wind_u"].defaulted())
        surf_wind->set_component_0_variable(
            opt_vals["surface_wind_u"].as<string>());

    if (!opt_vals["surface_wind_v"].defaulted())
        surf_wind->set_component_1_variable(
            opt_vals["surface_wind_v"].as<string>());

    if (!opt_vals["500mb_temp"].defaulted())
        core_temp->set_dependent_variable(0,
            opt_vals["500mb_temp"].as<string>());

    if (!opt_vals["200mb_temp"].defaulted())
        core_temp->set_dependent_variable(1,
            opt_vals["200mb_temp"].as<string>());

    if (!opt_vals["1000mb_height"].defaulted())
        thickness->set_dependent_variable(0,
            opt_vals["1000mb_height"].as<string>());

    if (!opt_vals["200mb_height"].defaulted())
        thickness->set_dependent_variable(1,
            opt_vals["200mb_height"].as<string>());

    if (!opt_vals["sea_level_pressure"].defaulted())
        candidates->set_sea_level_pressure_variable(
            opt_vals["sea_level_pressure"].as<string>());

    if (!opt_vals["storm_core_radius"].defaulted())
        candidates->set_max_core_radius(
            opt_vals["storm_core_radius"].as<double>());

    if (!opt_vals["min_vorticity"].defaulted())
        candidates->set_min_vorticity_850mb(
            opt_vals["min_vorticity"].as<double>());

    if (!opt_vals["vorticity_window"].defaulted())
        candidates->set_vorticity_850mb_window(
            opt_vals["vorticity_window"].as<double>());

    if (!opt_vals["pressure_delta"].defaulted())
        candidates->set_max_pressure_delta(
            opt_vals["pressure_delta"].as<double>());

    if (!opt_vals["pressure_delta_radius"].defaulted())
        candidates->set_max_pressure_radius(
            opt_vals["pressure_delta_radius"].as<double>());

    if (!opt_vals["core_temp_delta"].defaulted())
        candidates->set_max_core_temperature_delta(
            opt_vals["core_temp_delta"].as<double>());

    if (!opt_vals["core_temp_radius"].defaulted())
        candidates->set_max_core_temperature_radius(
            opt_vals["core_temp_radius"].as<double>());

    if (!opt_vals["thickness_delta"].defaulted())
        candidates->set_max_thickness_delta(
            opt_vals["thickness_delta"].as<double>());

    if (!opt_vals["thickness_radius"].defaulted())
        candidates->set_max_thickness_radius(
            opt_vals["thickness_radius"].as<double>());

    if (!opt_vals["lowest_lat"].defaulted())
        candidates->set_search_lat_low(
            opt_vals["lowest_lat"].as<double>());

    if (!opt_vals["highest_lat"].defaulted())
        candidates->set_search_lat_high(
            opt_vals["highest_lat"].as<double>());

    if (!opt_vals["first_step"].defaulted())
        map_reduce->set_start_index(opt_vals["first_step"].as<long>());

    if (!opt_vals["last_step"].defaulted())
        map_reduce->set_end_index(opt_vals["last_step"].as<long>());

    if (!opt_vals["n_threads"].defaulted())
        map_reduce->set_thread_pool_size(opt_vals["n_threads"].as<int>());
    else
        map_reduce->set_thread_pool_size(-1);

    if (!opt_vals["max_daily_distance"].defaulted())
        tracks->set_max_daily_distance(
            opt_vals["max_daily_distance"].as<double>());

    if (!opt_vals["min_wind_speed"].defaulted())
        tracks->set_min_wind_speed(
            opt_vals["min_wind_speed"].as<double>());

    if (!opt_vals["min_wind_duration"].defaulted())
        tracks->set_min_wind_duration(
            opt_vals["min_wind_duration"].as<double>());

    candidate_writer->set_file_name(
        opt_vals["candidate_file"].as<string>());

    track_writer->set_file_name(
        opt_vals["track_file"].as<string>());

    // some minimal check for missing options
    if ((have_file && have_regex) || !(have_file || have_regex))
    {
        if (mpi_man.get_comm_rank() == 0)
        {
            TECA_FATAL_ERROR("Extacly one of --input_file or --input_regex can be specified. "
                "Use --input_file to activate the multi_cf_reader (HighResMIP datasets) "
                "and --input_regex to activate the cf_reader (CAM like datasets)")
        }
        return -1;
    }

    // now that command line opts have been parsed we can create
    // the programmable algorithms' functors
    size_t n_var = core_temp->get_number_of_dependent_variables();
    if (n_var != 2)
    {
        TECA_FATAL_ERROR("core temperature calculation requires 2 "
            "variables. given " << n_var)
        return -1;
    }
    core_temp->set_execute_callback(
        point_wise_average(
            core_temp->get_dependent_variable(0),
            core_temp->get_dependent_variable(1),
            core_temp->get_derived_variable()));

    n_var = thickness->get_number_of_dependent_variables();
    if (n_var != 2)
    {
        TECA_FATAL_ERROR("thickness calculation requires 2 "
            "variables. given " << n_var)
        return -1;
    }
    thickness->set_execute_callback(
        point_wise_difference(
            thickness->get_dependent_variable(0),
            thickness->get_dependent_variable(1),
            thickness->get_derived_variable()));

    // and tell the candidate stage what variables the functors produce
    candidates->set_surface_wind_speed_variable(surf_wind->get_l2_norm_variable());
    candidates->set_vorticity_850mb_variable(vort_850mb->get_vorticity_variable());
    candidates->set_core_temperature_variable(core_temp->get_derived_variable());
    candidates->set_thickness_variable(thickness->get_derived_variable());

    // look for requested time step range, start
    bool parse_start_date = opt_vals.count("start_date");
    bool parse_end_date = opt_vals.count("end_date");
    if (parse_start_date || parse_end_date)
    {
        // run the reporting phase of the pipeline
        teca_metadata md = reader->update_metadata();

        teca_metadata atrs;
        if (md.get("attributes", atrs))
        {
            TECA_FATAL_ERROR("metadata mising attributes")
            return -1;
        }

        teca_metadata time_atts;
        std::string calendar;
        std::string units;
        if (atrs.get("time", time_atts)
           || time_atts.get("calendar", calendar)
           || time_atts.get("units", units))
        {
            TECA_FATAL_ERROR("failed to determine the calendaring parameters")
            return -1;
        }

        teca_metadata coords;
        p_teca_variant_array time;
        if (md.get("coordinates", coords) || !(time = coords.get("t")))
        {
            TECA_FATAL_ERROR("failed to determine time coordinate")
            return -1;
        }

        // convert date string to step, start date
        if (parse_start_date)
        {
            unsigned long first_step = 0;
            std::string start_date = opt_vals["start_date"].as<string>();
            if (teca_coordinate_util::time_step_of(time, true, true, calendar,
                 units, start_date, first_step))
            {
                TECA_FATAL_ERROR("Failed to lcoate time step for start date \""
                    <<  start_date << "\"")
                return -1;
            }
            map_reduce->set_start_index(first_step);
        }

        // and end date
        if (parse_end_date)
        {
            unsigned long last_step = 0;
            std::string end_date = opt_vals["end_date"].as<string>();
            if (teca_coordinate_util::time_step_of(time, false, true, calendar,
                 units, end_date, last_step))
            {
                TECA_FATAL_ERROR("Failed to lcoate time step for end date \""
                    <<  end_date << "\"")
                return -1;
            }
            map_reduce->set_end_index(last_step);
        }
    }

    // connect all the stages
    sim_coords->set_input_connection(reader->get_output_port());
    surf_wind->set_input_connection(sim_coords->get_output_port());
    vort_850mb->set_input_connection(surf_wind->get_output_port());
    core_temp->set_input_connection(vort_850mb->get_output_port());
    thickness->set_input_connection(core_temp->get_output_port());
    candidates->set_input_connection(thickness->get_output_port());
    map_reduce->set_input_connection(candidates->get_output_port());
    candidate_writer->set_input_connection(map_reduce->get_output_port());
    sort->set_input_connection(candidate_writer->get_output_port());
    tracks->set_input_connection(sort->get_output_port());
    calendar->set_input_connection(tracks->get_output_port());
    track_writer->set_input_connection(calendar->get_output_port());

    // run the pipeline
    track_writer->update();

    return 0;
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/toint.hpp>
#include <boost/simd/function/fast.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" toint real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::toint;
  using r_t = decltype(toint(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(toint(T(0.4)), r_t(0));
  STF_EQUAL(toint(T(0.5)), r_t(0));
  STF_EQUAL(toint(T(0.6)), r_t(0));
  STF_EQUAL(toint(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(toint(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(toint(bs::Zero<T>()), bs::Zero<r_t>());

} // end of test for floating_

STF_CASE_TPL (" toint unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::toint;
  using r_t = decltype(toint(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(toint(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(toint(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" toint signed",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::toint;
  using r_t = decltype(toint(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(toint(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(toint(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(toint(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for signed_int_


#define BOOST_TEST_MODULE "Test Groups class"

#include <boost/test/unit_test.hpp>
#include <cstdio>
#include "IO.hpp"
#include "distance/Euclidean.hpp"
#include "distance/Distance.hpp"
#include "Exception.hpp"
#include "group/Group.hpp"

using namespace genex;

struct MockData
{
  data_t dat_1[10] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};

  data_t dat_2[10] = {1, 1, 2, 2, 3, 3, 4, 4, 5, 5};

  data_t centroid_1[5] = {1, 1.5, 2.5, 3, 4};
  data_t centroid_2[10] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.5};

  data_t dat_3[10] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
  data_t dat_4[10] = {10, 9, 8, 7, 6, 5, 4, 3, 2, 1};
  data_t dat_5[10] = {9, 9, 8, 7, 6, 5, 4, 3, 2, 1};

  data_t dat_sim_1[5] = {1, 2, 3, 4, 5};
  data_t dat_sim_2[5] = {2, 3, 4, 5, 6};
  data_t dat_diff[5]  = {12, 13, 14, 15};

  std::string test_5_10_space = "datasets/test/test_5_10_space.txt";
  std::string test_3_10_space = "datasets/test/test_3_10_space.txt";
};

BOOST_AUTO_TEST_CASE( basic_groups, *boost::unit_test::tolerance(EPS) )
{
  MockData data;

  int timeSeriesCount = 5;
  int timeSeriesLengths = 10;
  int memberLength = 5;
  int subTimeSeriesCount = timeSeriesLengths - memberLength + 1;

  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_5_10_space, timeSeriesCount, 0, " ");

  BOOST_CHECK_EQUAL( tsSet.getMaxLength(), timeSeriesLengths );
  BOOST_CHECK_EQUAL( tsSet.getItemCount(), timeSeriesCount );
  BOOST_CHECK( tsSet.getFilePath() == data.test_5_10_space );

  std::vector<group_membership_t> memberMap =
    std::vector<group_membership_t>(tsSet.getItemCount() * tsSet.getMaxLength());
  Group g(0, memberLength, subTimeSeriesCount, tsSet, memberMap);

  const TimeSeries& c = g.getCentroid();

  // test initial centroid is all zeros
  for (int i = 0; i < memberLength; i++)
  {
    BOOST_TEST( c[i] == 0 );
  }

  g.addMember(0, 0);
  g.setCentroid(0, 0);

  //checking if centroid is updated
  BOOST_CHECK_EQUAL( g.getCount(), 1 );
  for (int i = 0; i < memberLength; i++)
  {
    BOOST_TEST( c[i] == data.dat_1[i] );
  }

  g.addMember(1, 0);

  BOOST_CHECK_EQUAL( g.getCount(), 2 );
}

BOOST_AUTO_TEST_CASE( group_get_best_match, *boost::unit_test::tolerance(EPS) )
{
  MockData data;
  dist_t distance = warpedDistance<Euclidean, data_t>;

  int timeSeriesCount = 3;
  int timeSeriesLengths = 10;
  int memberLength = 10;
  int subTimeSeriesCount = timeSeriesLengths - memberLength + 1;

  TimeSeriesSet tsSet;
  tsSet.loadData(data.test_3_10_space, timeSeriesCount, 0, " ");

  std::vector<group_membership_t> memberMap =
    std::vector<group_membership_t>(tsSet.getItemCount() * tsSet.getMaxLength());

  Group g(0, memberLength, subTimeSeriesCount, tsSet, memberMap);
  g.addMember(2, 0);
  g.addMember(0, 0);
  TimeSeries t = tsSet.getTimeSeries(1,0,memberLength);
  BOOST_TEST(t[0] == 1.0);
  candidate_time_series_t best = g.getBestMatch(t, distance);
  BOOST_TEST(best.dist == sqrt(1.0)/(2 * 10.0));
}

BOOST_AUTO_TEST_CASE( group_save_and_load , *boost::unit_test::tolerance(EPS) )
{
  // Set up
  std::string fname = "group_save_and_load.z";
  MockData data;
  auto dataset = TimeSeriesSet();
  dataset.loadData(data.test_3_10_space, 0, 0, " ");
  int length = 4;
  int subTimeSeriesCount = dataset.getMaxLength() - length + 1;
  auto memberMap = std::vector<group_membership_t>(dataset.getItemCount() * subTimeSeriesCount);
  auto group = new Group(1, length, subTimeSeriesCount, dataset, memberMap);
  group->setCentroid(1, 2);
  group->addMember(0, 0);
  group->addMember(2, 4);

  // Save
  saveToFile(*group, fname);

  // Load
  auto memberMap2 = std::vector<group_membership_t>(dataset.getItemCount() * subTimeSeriesCount);
  auto group2 = new Group(1, length, subTimeSeriesCount, dataset, memberMap2);
  loadFromFile(*group2, fname);

  // Compare
  BOOST_CHECK_EQUAL( group->getCount(), group2->getCount() );
  BOOST_CHECK_EQUAL( group->getMemberLength(), group2->getMemberLength() );
  BOOST_CHECK_EQUAL( group->getCentroid(), group2->getCentroid() );

  for (auto ts : group->getMembers()) {
    bool ok = false;
    for (auto ts2 : group2->getMembers()) {
      if (ts == ts2) {
        ok = true;
        break;
      }
    }
    BOOST_TEST( ok );
  }
  remove(fname.c_str());
  delete group;
  delete group2;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_ADJACENT_DIFFERENCE_HPP
#define BOOST_COMPUTE_ALGORITHM_ADJACENT_DIFFERENCE_HPP

#include <iterator>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/detail/meta_kernel.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/functional/operator.hpp>
#include <boost/compute/container/vector.hpp>

namespace boost {
namespace compute {

namespace detail {

template<class InputIterator, class OutputIterator, class BinaryFunction>
inline OutputIterator
dispatch_adjacent_difference(InputIterator first,
                             InputIterator last,
                             OutputIterator result,
                             BinaryFunction op,
                             command_queue &queue = system::default_queue())
{
    size_t count = detail::iterator_range_size(first, last);
    detail::meta_kernel k("adjacent_difference");

    k << "const uint i = get_global_id(0);\n"
      << "if(i == 0){\n"
      << "    " << result[k.var<uint_>("0")] << " = " << first[k.var<uint_>("0")] << ";\n"
      << "}\n"
      << "else {\n"
      << "    " << result[k.var<uint_>("i")] << " = "
      <<               op(first[k.var<uint_>("i")], first[k.var<uint_>("i-1")]) << ";\n"
      << "}\n";

    k.exec_1d(queue, 0, count, 1);

    return result + count;
}

} // end detail namespace

/// Stores the difference of each pair of consecutive values in the range
/// [\p first, \p last) to the range beginning at \p result. If \p op is not
/// provided, \c minus<T> is used.
///
/// \param first first element in the input range
/// \param last last element in the input range
/// \param result first element in the output range
/// \param op binary difference function
/// \param queue command queue to perform the operation
///
/// \return \c OutputIterator to the end of the result range
///
/// \see adjacent_find()
template<class InputIterator, class OutputIterator, class BinaryFunction>
inline OutputIterator
adjacent_difference(InputIterator first,
                    InputIterator last,
                    OutputIterator result,
                    BinaryFunction op,
                    command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<InputIterator>::value_type value_type;

    if(first == last) {
        return result;
    }

    if (first == result) {
        vector<value_type> temp(detail::iterator_range_size(first, last),
                                queue.get_context());
        copy(first, last, temp.begin(), queue);

        return ::boost::compute::detail::dispatch_adjacent_difference(
            temp.begin(), temp.end(), result, op, queue
        );
    }
    else {
        return ::boost::compute::detail::dispatch_adjacent_difference(
            first, last, result, op, queue
        );
    }
}

/// \overload
template<class InputIterator, class OutputIterator>
inline OutputIterator
adjacent_difference(InputIterator first,
                    InputIterator last,
                    OutputIterator result,
                    command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<InputIterator>::value_type value_type;

    return ::boost::compute::adjacent_difference(
        first, last, result, ::boost::compute::minus<value_type>(), queue
    );
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_ADJACENT_DIFFERENCE_HPP


#include <chrono>
#include <iostream>
#include <boost/program_options.hpp>
#include "trajec.h"
#include "simulate.h"
#include "moments.h"
#include "cim.h"
#include "tkf.h"

using namespace TrajectoryLikelihood;
using namespace std;
namespace po = boost::program_options;

int main (int argc, char** argv) {
  po::options_description opts ("Options");
  opts.add_options()
    ("help,h", "display this help message")
    ("verbose,v", po::value<int>()->default_value(0), "logging verbosity")
    ("lambda,L", po::value<double>(), "insertion rate (default is same as mu)")
    ("mu,M", po::value<double>()->default_value(.049), "deletion rate")
    ("x,X", po::value<double>(), "insertion extension probability (default is same as y)")
    ("y,Y", po::value<double>()->default_value(.543), "deletion extension probability")
    ("time,t", po::value<double>()->default_value(1), "time parameter")
    ("maxevents,E", po::value<int>()->default_value(3), "max # of indel events in trajectory")
    ("maxlen,l", po::value<int>()->default_value(10), "max length of chop zone")
    ("benchmark,b", "perform time benchmark instead of likelihood calculation")
    ("simulate,s", "perform stochastic simulation instead of likelihood calculation")
    ("counts,c", "report simulation counts instead of probabilities")
    ("initlen,i", po::value<int>()->default_value(1000), "initial sequence length for simulation")
    ("trials,n", po::value<int>()->default_value(100000), "number of simulation trials")
    ("moments,m", "use method of moments (Holmes 2020) for likelihood calculations")
    ("cim,C", "use De Maio 2020 Cumulative Indel Model for likelihood calculations")
    ("tkf91", "use TKF91 Pair HMM for likelihood calculations")
    ("tkf92", "use TKF92 Pair HMM for likelihood calculations")
    ("rs07", "use RS07 Pair HMM for likelihood calculations")
    ("prank", "use PRANK Pair HMM for likelihood calculations")
    ("dt,D", po::value<double>()->default_value(.01), "time step for numerical integration")
    ("seed,d", po::value<unsigned long long>()->default_value(mt19937::default_seed), "seed for random number generator")
    ("seedtime,T", "use current time as a seed");

    po::variables_map vm;
    po::parsed_options parsed = po::command_line_parser(argc,argv).options(opts).run();
    po::store (parsed, vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << opts << endl;
      return EXIT_SUCCESS;
    }

    // logging
    const int verbose = vm.at("verbose").as<int>();

    // get General Geometric Indel Model params (De Maio 2020; Holmes 2020)
    const double mu_ggi = vm.at("mu").as<double>();
    const double lambda_ggi = vm.count("lambda") ? vm.at("lambda").as<double>() : mu_ggi;
    const double rDel = vm.at("y").as<double>();
    const double rIns = vm.count("x") ? vm.at("x").as<double>() : rDel;

    // convert params to Long Indel Model scaling (Miklos, Lunter & Holmes 2004)
    const double mu_li = mu_ggi / (1. - rDel);
    const double gamma_li = (lambda_ggi * (1. - rIns)) / (mu_ggi * (1. - rDel));
    const IndelParams params (gamma_li, mu_li, rDel, rIns);

    if (verbose) {
      // log params in GGI format
      cerr << "mu_GGI=" << params.totalRightwardDeletionRatePerSite() << " lambda_GGI=" << params.totalInsertionRatePerSite() << " x=" << params.rIns << " y=" << params.rDel << endl;
    }

    // get remaining params
    const double t = vm.at("time").as<double>();

    const bool reportCounts = vm.count("simulate") && vm.count("counts");
    const int maxLen = vm.at("maxlen").as<int>();
    const double dt = vm.at("dt").as<double>();
    vector<vector<double> > probs;

    if (vm.count("benchmark")) {
      const int E = vm.at("maxevents").as<int>();
      cout << "chopZoneLength meanTime/uS error" << endl;
      const int trials = vm.at("trials").as<int>();
      for (int L = 1; L <= maxLen; ++L) {
	double tSum = 0, tSum2 = 0;
	for (int n = 0; n < trials; ++n) {
	  const auto tStart = std::chrono::system_clock::now();
	  const ChopZoneConfig config (E, L, verbose);
	  (void) chopZoneLikelihoods (params, t, config);
	  const auto tEnd = std::chrono::system_clock::now();
	  const double t = std::chrono::duration_cast< std::chrono::microseconds >(tEnd - tStart).count();
	  tSum += t;
	  tSum2 += t*t;
	}
	const double tMean = tSum / (double) trials, t2Mean = tSum2 / (double) trials;
	cout << L << " " << tMean << " " << sqrt ((t2Mean - tMean*tMean) / (double) trials) << endl;
      }
    } else {

      if (vm.count("simulate")) {
	const SimulationConfig config (vm.at("initlen").as<int>(), maxLen, vm.at("trials").as<int>(), verbose);
	const bool useClock = vm.count("seedtime");
	unsigned long long seed;
	if (useClock)
	  seed = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()).count();
	else
	  seed = vm.at("seed").as<unsigned long long>();
	cerr << "Random number seed: " << seed << endl;
	mt19937 rnd (seed);
	probs = chopZoneSimulatedProbabilities (params, t, config, rnd, reportCounts);
      } else if (vm.count("moments")) {
	const Moments moments (params, t, dt, verbose);
	probs = moments.chopZoneLikelihoods (maxLen);
      } else if (vm.count("cim")) {
	const CumulativeIndelModel cim (params, t, dt, verbose);
	probs = cim.chopZoneLikelihoods (maxLen);
      } else if (vm.count("tkf91")) {
	const TKF91 tkf91 (params, t, verbose);
	probs = tkf91.chopZoneLikelihoods (maxLen);
      } else if (vm.count("tkf92")) {
	const TKF92 tkf92 (params, t, verbose);
	probs = tkf92.chopZoneLikelihoods (maxLen);
      } else if (vm.count("prank")) {
	const PRANK prank (params, t, verbose);
	probs = prank.chopZoneLikelihoods (maxLen);
      } else if (vm.count("rs07")) {
	const RS07 rs07 (params, t, verbose);
	probs = rs07.chopZoneLikelihoods (maxLen);
      } else {
	const ChopZoneConfig config (vm.at("maxevents").as<int>(), maxLen, verbose);
	probs = chopZoneLikelihoods (params, t, config);
      }

      double total = 0;
      for (const auto& pd: probs) {
	for (double p: pd)
	  total += p;
	cout << to_string_join (pd) << endl;
      }
      if (verbose) {
	cerr << "Entry in row i, column j is " << (reportCounts ? "count" : "probability") << " of inserting i residues and deleting j residues before the next match" << endl;
	if (reportCounts)
	  cerr << "Final count represents overflow (chop zones that were larger than size limit)" << endl;
	cerr << "Total: " << total << endl;
	if (!vm.count("counts")) {
	  double ei = 0, ed = 0, ei2 = 0, ed2 = 0, eid = 0, pi0 = 0, pd0 = 0;
	  for (int i = 0; i < probs.size(); ++i)
	    for (int d = 0; d < probs[i].size(); ++d) {
	      const double p = probs[i][d];
	      ei += i * p;
	      ed += d * p;
	      ei2 += i * i * p;
	      ed2 += d * d * p;
	      eid += i * d * p;
	      if (i == 0)
		pi0 += p;
	      if (d == 0)
		pd0 += p;
	    }
	  cerr << "E[#ins]=" << ei << " E[#del]=" << ed
	       << " E[#ins^2]=" << ei2 << " E[#del^2]=" << ed2
	       << " V[#ins]=" << (ei2 - ei*ei) << " V[#del]=" << (ed2 - ed*ed)
	       << " E[#ins*#del]=" << eid
	       << " Cov(#ins,#del)=" << (eid-ei*ed)
	       << endl;
	}
      }
    }

    return EXIT_SUCCESS;
}


/**
 * @file ipdgfem_main.cc
 * @brief NPDE homework IPDGFEM code
 * @author Philippe Peter
 * @date 22.11.2019
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>

#include "ipdgfem.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  Eigen::VectorXd v = IPDGFEM::dummyFunction(0.0, 0);
  std::cout << v.transpose().format(CSVFormat) << std::endl;

  return 0;
}


#include <boost/utility/enable_if.hpp>
#include <type_traits>
#include <iostream>

template <typename T>
void print(typename boost::enable_if<std::is_integral<T>, T>::type i)
{
  std::cout << "Integral: " << i << '\n';
}

template <typename T>
void print(typename boost::enable_if<std::is_floating_point<T>, T>::type f)
{
  std::cout << "Floating point: " << f << '\n';
}

int main()
{
  print<short>(1);
  print<long>(2);
  print<double>(3.14);
}


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <iostream>
#include <iomanip>
#include <iterator>
#include <map>
#include <set>
#include <fstream>
#include <string>
#include <algorithm>
#include <functional>
#include <dai/factorgraph.h>
#include <dai/util.h>
#include <dai/exceptions.h>
#include <boost/lexical_cast.hpp>

namespace dai {

using namespace std;

FactorGraph::FactorGraph( const std::vector<Factor> &P ) : _G(), _backup() {
    // add factors, obtain variables
    set<Var> varset;
    _factors.reserve( P.size() );
    size_t nrEdges = 0;
    for( vector<Factor>::const_iterator p2 = P.begin(); p2 != P.end(); p2++ ) {
        _factors.push_back( *p2 );
        copy( p2->vars().begin(), p2->vars().end(), inserter( varset, varset.begin() ) );
        nrEdges += p2->vars().size();
    }

    // add vars
    _vars.reserve( varset.size() );
    for( set<Var>::const_iterator p1 = varset.begin(); p1 != varset.end(); p1++ )
        _vars.push_back( *p1 );

    // create graph structure
    constructGraph( nrEdges );
}

void FactorGraph::constructGraph( size_t nrEdges ) {
    // create a mapping for indices
    hash_map<size_t, size_t> hashmap;

    for( size_t i = 0; i < vars().size(); i++ )
        hashmap[var(i).label()] = i;

    // create edge list
    vector<Edge> edges;
    edges.reserve( nrEdges );
    for( size_t i2 = 0; i2 < nrFactors(); i2++ ) {
        const VarSet& ns = factor(i2).vars();
        for( VarSet::const_iterator q = ns.begin(); q != ns.end(); q++ )
            edges.push_back( Edge(hashmap[q->label()], i2) );
    }

    // create bipartite graph
    _G.construct( nrVars(), nrFactors(), edges.begin(), edges.end() );
}

/// Writes a FactorGraph to an output stream
std::ostream& operator<< ( std::ostream &os, const FactorGraph &fg ) {
    os << fg.nrFactors() << endl;

    for( size_t I = 0; I < fg.nrFactors(); I++ ) {
        os << endl;
        os << fg.factor(I).vars().size() << endl;
        for( VarSet::const_iterator i = fg.factor(I).vars().begin(); i != fg.factor(I).vars().end(); i++ )
            os << i->label() << " ";
        os << endl;
        for( VarSet::const_iterator i = fg.factor(I).vars().begin(); i != fg.factor(I).vars().end(); i++ )
            os << i->states() << " ";
        os << endl;
        size_t nr_nonzeros = 0;
        for( size_t k = 0; k < fg.factor(I).nrStates(); k++ )
            if( fg.factor(I)[k] != (Real)0 )
                nr_nonzeros++;
        os << nr_nonzeros << endl;
        for( size_t k = 0; k < fg.factor(I).nrStates(); k++ )
            if( fg.factor(I)[k] != (Real)0 )
                os << k << " " << setw(os.precision()+4) << fg.factor(I)[k] << endl;
    }

    return(os);
}

/// Reads a FactorGraph from an input stream
std::istream& operator>> ( std::istream& is, FactorGraph &fg ) {
    long verbose = 0;

    vector<Factor> facs;
    size_t nr_Factors;
    string line;

    while( (is.peek()) == '#' )
        getline(is,line);
    is >> nr_Factors;
    if( is.fail() )
        DAI_THROWE(INVALID_FACTORGRAPH_FILE,"Cannot read number of factors");
    if( verbose >= 1 )
        cerr << "Reading " << nr_Factors << " factors..." << endl;

    getline (is,line);
    if( is.fail() || line.size() > 0 )
        DAI_THROWE(INVALID_FACTORGRAPH_FILE,"Expecting empty line");

    map<long,size_t> vardims;
    for( size_t I = 0; I < nr_Factors; I++ ) {
        if( verbose >= 2 )
            cerr << "Reading factor " << I << "..." << endl;
        size_t nr_members;
        while( (is.peek()) == '#' )
            getline(is,line);
        is >> nr_members;
        if( verbose >= 2 )
            cerr << "  nr_members: " << nr_members << endl;

        vector<long> labels;
        for( size_t mi = 0; mi < nr_members; mi++ ) {
            long mi_label;
            while( (is.peek()) == '#' )
                getline(is,line);
            is >> mi_label;
            labels.push_back(mi_label);
        }
        if( verbose >= 2 )
            cerr << "  labels: " << labels << endl;

        vector<size_t> dims;
        for( size_t mi = 0; mi < nr_members; mi++ ) {
            size_t mi_dim;
            while( (is.peek()) == '#' )
                getline(is,line);
            is >> mi_dim;
            dims.push_back(mi_dim);
        }
        if( verbose >= 2 )
            cerr << "  dimensions: " << dims << endl;

        // add the Factor
        vector<Var> Ivars;
        Ivars.reserve( nr_members );
        for( size_t mi = 0; mi < nr_members; mi++ ) {
            map<long,size_t>::iterator vdi = vardims.find( labels[mi] );
            if( vdi != vardims.end() ) {
                // check whether dimensions are consistent
                if( vdi->second != dims[mi] )
                    DAI_THROWE(INVALID_FACTORGRAPH_FILE,"Variable with label " + boost::lexical_cast<string>(labels[mi]) + " has inconsistent dimensions.");
            } else
                vardims[labels[mi]] = dims[mi];
            Ivars.push_back( Var(labels[mi], dims[mi]) );
        }
        facs.push_back( Factor( VarSet( Ivars.begin(), Ivars.end(), Ivars.size() ), (Real)0 ) );
        if( verbose >= 2 )
            cerr << "  vardims: " << vardims << endl;

        // calculate permutation object
        Permute permindex( Ivars );

        // read values
        size_t nr_nonzeros;
        while( (is.peek()) == '#' )
            getline(is,line);
        is >> nr_nonzeros;
        if( verbose >= 2 )
            cerr << "  nonzeroes: " << nr_nonzeros << endl;
        for( size_t k = 0; k < nr_nonzeros; k++ ) {
            size_t li;
            Real val;
            while( (is.peek()) == '#' )
                getline(is,line);
            is >> li;
            while( (is.peek()) == '#' )
                getline(is,line);
            is >> val;

            // store value, but permute indices first according to internal representation
            facs.back().set( permindex.convertLinearIndex( li ), val );
        }
    }

    if( verbose >= 3 )
        cerr << "factors:" << facs << endl;

    fg = FactorGraph(facs);

    return is;
}

VarSet FactorGraph::Delta( size_t i ) const {
    // calculate Markov Blanket
    VarSet Del;
    for( const Neighbor &I : nbV(i) ) // for all neighboring factors I of i
        for( const Neighbor &j : nbF(I) ) // for all neighboring variables j of I
            Del |= var(j);

    return Del;
}

VarSet FactorGraph::Delta( const VarSet &ns ) const {
    VarSet result;
    for( VarSet::const_iterator n = ns.begin(); n != ns.end(); n++ )
        result |= Delta( findVar(*n) );
    return result;
}

SmallSet<size_t> FactorGraph::Deltai( size_t i ) const {
    // calculate Markov Blanket
    SmallSet<size_t> Del;
    for( const Neighbor &I : nbV(i) ) // for all neighboring factors I of i
        for( const Neighbor &j : nbF(I) ) // for all neighboring variables j of I
            Del |= j;

    return Del;
}

void FactorGraph::makeCavity( size_t i, bool backup ) {
    // fills all Factors that include var(i) with ones
    map<size_t,Factor> newFacs;
    for( const Neighbor &I : nbV(i) ) // for all neighboring factors I of i
        newFacs[I] = Factor( factor(I).vars(), (Real)1 );
    setFactors( newFacs, backup );
}

void FactorGraph::makeRegionCavity( std::vector<size_t> facInds, bool backup ) {
    map<size_t,Factor> newFacs;
    for( size_t I = 0; I < facInds.size(); I++ )
        newFacs[facInds[I]] = Factor(factor(facInds[I]).vars(), (Real)1);
    setFactors( newFacs, backup );
}

void FactorGraph::ReadFromFile( const char *filename ) {
    ifstream infile;
    infile.open( filename );
    if( infile.is_open() ) {
        infile >> *this;
        infile.close();
    } else
        DAI_THROWE(CANNOT_READ_FILE,"Cannot read from file " + std::string(filename));
}

void FactorGraph::WriteToFile( const char *filename, size_t precision ) const {
    ofstream outfile;
    outfile.open( filename );
    if( outfile.is_open() ) {
        outfile.precision( precision );
        outfile << *this;
        outfile.close();
    } else
        DAI_THROWE(CANNOT_WRITE_FILE,"Cannot write to file " + std::string(filename));
}

void FactorGraph::printDot( std::ostream &os ) const {
    os << "graph FactorGraph {" << endl;
    os << "node[shape=circle,width=0.4,fixedsize=true];" << endl;
    for( size_t i = 0; i < nrVars(); i++ )
        os << "\tv" << var(i).label() << ";" << endl;
    os << "node[shape=box,width=0.3,height=0.3,fixedsize=true];" << endl;
    for( size_t I = 0; I < nrFactors(); I++ )
        os << "\tf" << I << ";" << endl;
    for( size_t i = 0; i < nrVars(); i++ )
        for( const Neighbor &I : nbV(i) )  // for all neighboring factors I of i
            os << "\tv" << var(i).label() << " -- f" << I << ";" << endl;
    os << "}" << endl;
}

GraphAL FactorGraph::MarkovGraph() const {
    GraphAL G( nrVars() );
    for( size_t i = 0; i < nrVars(); i++ )
        for( const Neighbor &I : nbV(i) )
            for( const Neighbor &j : nbF(I) )
                if( i < j )
                    G.addEdge( i, j, true );
    return G;
}

bool FactorGraph::isMaximal( size_t I ) const {
    const VarSet& I_vars = factor(I).vars();
    size_t I_size = I_vars.size();

    if( I_size == 0 ) {
        for( size_t J = 0; J < nrFactors(); J++ )
            if( J != I )
                if( factor(J).vars().size() > 0 )
                    return false;
        return true;
    } else {
        for( const Neighbor& i : nbF(I) ) {
            for( const Neighbor& J : nbV(i) ) {
                if( J != I )
                    if( (factor(J).vars() >> I_vars) && (factor(J).vars().size() != I_size) )
                        return false;
            }
        }
        return true;
    }
}

size_t FactorGraph::maximalFactor( size_t I ) const {
    const VarSet& I_vars = factor(I).vars();
    size_t I_size = I_vars.size();

    if( I_size == 0 ) {
        for( size_t J = 0; J < nrFactors(); J++ )
            if( J != I )
                if( factor(J).vars().size() > 0 )
                    return maximalFactor( J );
        return I;
    } else {
        for( const Neighbor& i : nbF(I) ) {
            for( const Neighbor& J : nbV(i) ) {
                if( J != I )
                    if( (factor(J).vars() >> I_vars) && (factor(J).vars().size() != I_size) )
                        return maximalFactor( J );
            }
        }
        return I;
    }
}

vector<VarSet> FactorGraph::maximalFactorDomains() const {
    vector<VarSet> result;

    for( size_t I = 0; I < nrFactors(); I++ )
        if( isMaximal( I ) )
            result.push_back( factor(I).vars() );

    if( result.size() == 0 )
        result.push_back( VarSet() );
    return result;
}

Real FactorGraph::logScore( const std::vector<size_t>& statevec ) const {
    // Construct a State object that represents statevec
    // This decouples the representation of the joint state in statevec from the factor graph
    map<Var, size_t> statemap;
    for( size_t i = 0; i < statevec.size(); i++ )
        statemap[var(i)] = statevec[i];
    State S(statemap);

    // Evaluate the log probability of the joint configuration in statevec
    // by summing the log factor entries of the factors that correspond to this joint configuration
    Real lS = 0.0;
    for( size_t I = 0; I < nrFactors(); I++ )
        lS += dai::log( factor(I)[BigInt_size_t(S(factor(I).vars()))] );
    return lS;
}

void FactorGraph::clamp( size_t i, size_t x, bool backup ) {
    DAI_ASSERT( x <= var(i).states() );
    Factor mask( var(i), (Real)0 );
    mask.set( x, (Real)1 );

    map<size_t, Factor> newFacs;
    for( const Neighbor &I : nbV(i) )
        newFacs[I] = factor(I) * mask;
    setFactors( newFacs, backup );

    return;
}

void FactorGraph::clampVar( size_t i, const vector<size_t> &is, bool backup ) {
    Var n = var(i);
    Factor mask_n( n, (Real)0 );

    for( size_t i : is ) {
        DAI_ASSERT( i <= n.states() );
        mask_n.set( i, (Real)1 );
    }

    map<size_t, Factor> newFacs;
    for( const Neighbor &I : nbV(i) )
        newFacs[I] = factor(I) * mask_n;
    setFactors( newFacs, backup );
}

void FactorGraph::clampFactor( size_t I, const vector<size_t> &is, bool backup ) {
#ifndef DAI_PERF
    size_t st = factor(I).nrStates();
#endif
    Factor newF( factor(I).vars(), (Real)0 );

    for( size_t i : is ) {
        DAI_ASSERT( i <= st );
        newF.set( i, factor(I)[i] );
    }

    setFactor( I, newF, backup );
}

void FactorGraph::backupFactor( size_t I ) {
    map<size_t,Factor>::iterator it = _backup.find( I );
    if( it != _backup.end() )
        DAI_THROW(MULTIPLE_UNDO);
    _backup[I] = factor(I);
}

void FactorGraph::restoreFactor( size_t I ) {
    map<size_t,Factor>::iterator it = _backup.find( I );
    if( it != _backup.end() ) {
        setFactor(I, it->second);
        _backup.erase(it);
    } else
        DAI_THROW(OBJECT_NOT_FOUND);
}

void FactorGraph::backupFactors( const VarSet &ns ) {
    for( size_t I = 0; I < nrFactors(); I++ )
        if( factor(I).vars().intersects( ns ) )
            backupFactor( I );
}

void FactorGraph::restoreFactors( const VarSet &ns ) {
    map<size_t,Factor> facs;
    for( map<size_t,Factor>::iterator uI = _backup.begin(); uI != _backup.end(); ) {
        if( factor(uI->first).vars().intersects( ns ) ) {
            facs.insert( *uI );
            _backup.erase(uI++);
        } else
            uI++;
    }
    setFactors( facs );
}

void FactorGraph::restoreFactors() {
    setFactors( _backup );
    _backup.clear();
}

void FactorGraph::backupFactors( const std::set<size_t> & facs ) {
    for( std::set<size_t>::const_iterator fac = facs.begin(); fac != facs.end(); fac++ )
        backupFactor( *fac );
}

bool FactorGraph::isPairwise() const {
    bool pairwise = true;
    for( size_t I = 0; I < nrFactors() && pairwise; I++ )
        if( factor(I).vars().size() > 2 )
            pairwise = false;
    return pairwise;
}

bool FactorGraph::isBinary() const {
    bool binary = true;
    for( size_t i = 0; i < nrVars() && binary; i++ )
        if( var(i).states() > 2 )
            binary = false;
    return binary;
}

FactorGraph FactorGraph::clamped( size_t i, size_t state ) const {
    Var v = var( i );
    Real zeroth_order = (Real)1;
    vector<Factor> clamped_facs;
    clamped_facs.push_back( createFactorDelta( v, state ) );
    for( size_t I = 0; I < nrFactors(); I++ ) {
        VarSet v_I = factor(I).vars();
        Factor new_factor;
        if( v_I.intersects( v ) )
            new_factor = factor(I).slice( v, state );
        else
            new_factor = factor(I);

        if( new_factor.vars().size() != 0 ) {
            size_t J = 0;
            // if it can be merged with a previous one, do that
            for( J = 0; J < clamped_facs.size(); J++ )
                if( clamped_facs[J].vars() == new_factor.vars() ) {
                    clamped_facs[J] *= new_factor;
                    break;
                }
            // otherwise, push it back
            if( J == clamped_facs.size() || clamped_facs.size() == 0 )
                clamped_facs.push_back( new_factor );
        } else
            zeroth_order *= new_factor[0];
    }
    *(clamped_facs.begin()) *= zeroth_order;
    return FactorGraph( clamped_facs );
}

FactorGraph FactorGraph::maximalFactors() const {
    vector<size_t> maxfac( nrFactors() );
    map<size_t,size_t> newindex;
    size_t nrmax = 0;
    for( size_t I = 0; I < nrFactors(); I++ ) {
        maxfac[I] = I;
        VarSet maxfacvars = factor(maxfac[I]).vars();
        for( size_t J = 0; J < nrFactors(); J++ ) {
            VarSet Jvars = factor(J).vars();
            if( Jvars >> maxfacvars && (Jvars != maxfacvars) ) {
                maxfac[I] = J;
                maxfacvars = factor(maxfac[I]).vars();
            }
        }
        if( maxfac[I] == I )
            newindex[I] = nrmax++;
    }

    vector<Factor> facs( nrmax );
    for( size_t I = 0; I < nrFactors(); I++ )
        facs[newindex[maxfac[I]]] *= factor(I);

    return FactorGraph( facs.begin(), facs.end(), vars().begin(), vars().end(), facs.size(), nrVars() );
}

} // end of namespace dai


#pragma once

#include <boost/spirit/home/x3/char/char_class.hpp>
#include <boost/spirit/home/x3/numeric/uint.hpp>
#include <boost/spirit/home/x3/string/literal_string.hpp>

namespace pdl::spirit::grammar::types {

constexpr auto bin = x3::lit("0b") > x3::bin;
constexpr auto hex = x3::lit("0x") > x3::hex;
constexpr auto byte = x3::xdigit >> x3::xdigit;
constexpr auto word = byte >> byte;
constexpr auto octet = x3::uint_parser<uint8_t, 10, 1, 3> { };

}  // namespace types.


#include "cv_bridge/cv_bridge.h"
#include <boost/scoped_ptr.hpp>
#include <gtest/gtest.h>
#include "opencv2/core/core.hpp"
#include <sstream>

using namespace cv;
using namespace boost;
using namespace std;

namespace cv_bridge {

class MatNDTest : public ::testing::Test {
 protected:
  Mat mat_;

  virtual void SetUp() {
    int size[] = {2,3,4};
    mat_ = Mat(3, size, CV_8U, Scalar::all(0));

    // Fill the matrix with the linear index number
    for (uint8_t i = 0; i < 24; ++i) {
      mat_.at<uint8_t>(0, 0, i) = i;
    }
  }
};

TEST_F(MatNDTest, RoundTripTestCopyNoConversion) {
  CvMultiMat startMat;
  startMat.mat = mat_;

  sensor_msgs::MatNDPtr msg = startMat.toMsg();

  CvMultiMatPtr matCopy = toCvCopy(msg);

  // Make sure the layout of the matrices is the same
  ASSERT_EQ(mat_.dims, matCopy->mat.dims);
  for (int i = 0; i < mat_.dims; ++i) {
    ASSERT_EQ(mat_.size[i], matCopy->mat.size[i]);
  }

  // Make sure all the values are the same
  MatConstIterator_<uint8_t> copyI = matCopy->mat.begin<uint8_t>();
  for (MatConstIterator_<uint8_t> startI = mat_.begin<uint8_t>();
       startI != mat_.end<uint8_t>();
       ++startI, ++copyI) {
    EXPECT_EQ(*startI, *copyI);
  }

  // Make sure that the locations are where I expect them
  EXPECT_EQ(mat_.at<uint8_t>(0, 1, 2), 6);
  EXPECT_EQ(mat_.at<uint8_t>(1, 0, 1), 13);
  EXPECT_EQ(mat_.at<uint8_t>(1, 2, 0), 20);

  // Make sure that a copy was made
  ASSERT_NE(&msg->data[0],
            matCopy->mat.data);
}

TEST_F(MatNDTest, RoundTripTest32BitInt) {
  CvMultiMat startMat;
  mat_.convertTo(startMat.mat, CV_32S);

  sensor_msgs::MatNDPtr msg = startMat.toMsg();

  CvMultiMatPtr matCopy = toCvCopy(msg);
  ASSERT_EQ(matCopy->mat.type(), CV_32S);

  // Make sure the layout of the matrices is the same
  ASSERT_EQ(mat_.dims, matCopy->mat.dims);
  for (int i = 0; i < mat_.dims; ++i) {
    ASSERT_EQ(mat_.size[i], matCopy->mat.size[i]);
  }

  // Make sure all the values are the same
  MatConstIterator_<int32_t> copyI = matCopy->mat.begin<int32_t>();
  for (MatConstIterator_<int32_t> startI = startMat.mat.begin<int32_t>();
       startI != startMat.mat.end<int32_t>();
       ++startI, ++copyI) {
    EXPECT_EQ(*startI, *copyI);
  }

  // Make sure that the locations are where I expect them
  EXPECT_EQ(matCopy->mat.at<int32_t>(0, 1, 2), 6);
  EXPECT_EQ(matCopy->mat.at<int32_t>(1, 0, 1), 13);
  EXPECT_EQ(matCopy->mat.at<int32_t>(1, 2, 0), 20);

  // Make sure that a copy was made
  ASSERT_NE(&msg->data[0],
            matCopy->mat.data);
}

TEST_F(MatNDTest, RoundTripTestShareNoConversion) {
  CvMultiMat startMat;
  startMat.mat = mat_;
  sensor_msgs::MatNDPtr msg = startMat.toMsg();

  CvMultiMatConstPtr matCopy = toCvShare(msg);

  // Make sure the layout of the matrices is the same
  ASSERT_EQ(mat_.dims, matCopy->mat.dims);
  for (int i = 0; i < mat_.dims; ++i) {
    ASSERT_EQ(mat_.size[i], matCopy->mat.size[i]);
  }

  // Make sure that the data is shared between the message and the copy
  ASSERT_EQ(&msg->data[0],
            matCopy->mat.data);
}

TEST_F(MatNDTest, RoundTripTestCopyConversion) {
  CvMultiMat startMat;
  startMat.mat = mat_;

  sensor_msgs::MatNDPtr msg = startMat.toMsg();

  CvMultiMatPtr matCopy = toCvCopy(msg, "32S");

  // Make sure the layout of the matrices is the same
  ASSERT_EQ(mat_.dims, matCopy->mat.dims);
  for (int i = 0; i < mat_.dims; ++i) {
    ASSERT_EQ(mat_.size[i], matCopy->mat.size[i]);
  }

  // Make sure all the values are the same
  MatConstIterator_<int> copyI = matCopy->mat.begin<int>();
  for (MatConstIterator_<uint8_t> startI = mat_.begin<uint8_t>();
       startI != mat_.end<uint8_t>();
       ++startI, ++copyI) {
    EXPECT_EQ(*startI, *copyI);
  }
}

TEST_F(MatNDTest, CheckMessageFormat) {
  CvMultiMat startMat;
  startMat.mat = mat_;

  sensor_msgs::MatNDPtr msg = startMat.toMsg();

  EXPECT_EQ(msg->sizes[0], 2);
  EXPECT_EQ(msg->sizes[1], 3);
  EXPECT_EQ(msg->sizes[2], 4);

  EXPECT_EQ(msg->encoding, string("8U"));

  EXPECT_EQ(msg->is_bigendian, false);

  ASSERT_EQ(msg->data.size(), 24u);
  for (uint32_t i = 0u; i < msg->data.size(); ++i) {
    EXPECT_EQ(msg->data[i], i);
  }
}

} // namespace

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#pragma once
#include <boost/multiprecision/cpp_int.hpp>
#include <fstream>

#include "../utils/printer.hpp"
#include "../utils/random.hpp"
#include "../utils/scanner.hpp"
namespace libtest {
struct a_multiply_b_large
{
    using bigint                            = boost::multiprecision::cpp_int;
    static constexpr const char* path       = "a_multiply_b_large";
    static constexpr const char* name       = "A*B (Large)";
    static constexpr std::size_t time_limit = 1000;
    template<typename constraints>
    static void generate_input(std::ofstream& input_file)
    {
        constexpr auto d_min = constraints::d_min, d_max = constraints::d_max;
        printer pr{input_file};
        std::string n[2];
        for (usize i = 0; i < 2; i++) {
            const auto d = rng.gen(d_min, d_max);
            if (rng.gen(0, 1) == 1) { n[i].push_back('-'); }
            for (usize j = 0; j < d; j++) {
                n[i].push_back(rng.gen(j == 0 ? '1' : '0', '9'));
            }
        }
        pr.println(n[0], n[1]);
    }
    static void generate_output(std::ifstream& input_file, std::ofstream& output_file)
    {
        scanner sc(input_file);
        printer pr(output_file);
        const auto [a, b] = sc.read_vals<bigint, bigint>();
        pr.println(a * b);
    }
    static bool judge(std::ifstream& /* input_file */, std::ifstream& generated_output_file, std::ifstream& solution_output_file)
    {
        scanner gen_sc(generated_output_file), sol_sc(solution_output_file);
        return gen_sc.read<std::string>() == sol_sc.may_read<std::string>();
    }
    struct small_constraints
    {
        static constexpr usize d_min = 1, d_max = 100;
    };
    struct large_constraints
    {
        static constexpr usize d_min = 1, d_max = 100000;
    };
};
}  // namespace libtest