/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <thread>

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

#include "tudat/astro/basic_astro/unitConversions.h"
#include "tudat/math/basic/linearAlgebra.h"
#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/astro/basic_astro/orbitalElementConversions.h"

#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/math/integrators/rungeKuttaCoefficients.h"
#include "tudat/astro/basic_astro/accelerationModel.h"
#include "tudat/io/basicInputOutput.h"
#include "tudat/simulation/propagation_setup/dynamicsSimulator.h"
#include "tudat/simulation/environment_setup/defaultBodies.h"
#include "tudat/simulation/environment_setup/createBodies.h"
#include "tudat/simulation/estimation_setup/createNumericalSimulator.h"
#include "tudat/math/integrators/createNumericalIntegrator.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_exact_termination )

//! Test exact termination conditions, to see if the propagation stops exactly (within tolerance) when it's supposed to.
//! The test is run for an RK4 and RK7(8) integrator, with otherwise identical settings.
//! Five types of termination conditions are used:
//! 0) Termination on exact time
//! 1) Termination on exact altitude
//! 2) Termination when _either_ an exact altitude _or_ an exact time is reached (whichever comes first), with the two occuring
//! very near one another
//! 3) Termination when _both_ an exact altitude _and_ an exact time are reached, with the two occuring very near one another
//! 4) Termination when _either_ an exact altitude _or_ an exact time is reached (whichever comes first), with the two _not_
//! occuring very near one another
//!
//! The tests are run for forward and backward propagation
BOOST_AUTO_TEST_CASE( testEnckePopagatorForSphericalHarmonicCentralBodies )
{
    for( unsigned int integratorCase = 0; integratorCase < 2; integratorCase++ )
    {
        for( unsigned int direction = 0; direction < 2; direction++ )
        {
            for( unsigned int simulationCase = 0; simulationCase < 5; simulationCase++ )
            {
                std::cout<<integratorCase<<" "<<direction<<" "<<simulationCase<<std::endl;
                using namespace tudat;
                using namespace simulation_setup;
                using namespace propagators;
                using namespace numerical_integrators;
                using namespace orbital_element_conversions;
                using namespace basic_mathematics;
                using namespace gravitation;

                // Load Spice kernels.
                spice_interface::loadStandardSpiceKernels( );

                // Set simulation time settings.
                double simulationStartEpoch;
                double simulationEndEpoch;

                double directionMultiplier = 1.0;
                if( direction == 0 )
                {
                    simulationStartEpoch = 0.0;
                    simulationEndEpoch = 0.2 * tudat::physical_constants::JULIAN_DAY;
                }
                else
                {
                    simulationStartEpoch = 0.2 * tudat::physical_constants::JULIAN_DAY;
                    simulationEndEpoch = 0.0;
                    directionMultiplier = -1.0;
                }

                // Define body settings for simulation.
                std::vector< std::string > bodiesToCreate;
                bodiesToCreate.push_back( "Sun" );
                bodiesToCreate.push_back( "Earth" );
                bodiesToCreate.push_back( "Moon" );

                // Create body objects.
                BodyListSettings bodySettings = BodyListSettings( "Earth", "ECLIPJ2000" );
                if( direction == 0 )
                {
                    bodySettings =
                            getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 );
                }
                else
                {
                    bodySettings =
                            getDefaultBodySettings( bodiesToCreate, simulationEndEpoch - 300.0, simulationStartEpoch + 300.0 );
                }
                SystemOfBodies bodies = createSystemOfBodies( bodySettings );

                // Create spacecraft object.
                bodies.createEmptyBody( "Vehicle" );
                bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 );

                // Define propagator settings variables.
                SelectedAccelerationMap accelerationMap;
                std::vector< std::string > bodiesToPropagate;
                std::vector< std::string > centralBodies;

                // Define propagation settings.
                std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;

                {
                    accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                                     basic_astrodynamics::point_mass_gravity ) );
                    accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                                   basic_astrodynamics::point_mass_gravity ) );
                    accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                                    basic_astrodynamics::point_mass_gravity ) );
                }

                accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;
                bodiesToPropagate.push_back( "Vehicle" );
                centralBodies.push_back( "Earth" );
                basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                            bodies, accelerationMap, bodiesToPropagate, centralBodies );

                // Set Keplerian elements for Vehicle.
                Eigen::Vector6d vehicleInitialStateInKeplerianElements;
                vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 8000.0E3;
                vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
                vehicleInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
                vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
                        = unit_conversions::convertDegreesToRadians( 235.7 );
                vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
                        = unit_conversions::convertDegreesToRadians( 23.4 );
                vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );

                double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
                const Eigen::Vector6d vehicleInitialState = convertKeplerianToCartesianElements(
                            vehicleInitialStateInKeplerianElements, earthGravitationalParameter );

                // Define propagator settings (Cowell)
                std::shared_ptr< PropagationTerminationSettings > terminationSettings;
                std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables;
                dependentVariables.push_back(
                            std::make_shared< SingleDependentVariableSaveSettings >( relative_distance_dependent_variable,
                                                                                "Vehicle", "Earth" ) );
                double finalTestTime;
                double secondFinalTestTime;

                if( direction == 0 )
                {
                    finalTestTime = 322.5;
                    secondFinalTestTime = 501.0;
                }
                else
                {
                    finalTestTime = 11737.5;
                    secondFinalTestTime = 11701.0;
                }
                if( simulationCase == 0 )
                {
                    terminationSettings = std::make_shared< PropagationTimeTerminationSettings >(
                                simulationEndEpoch - directionMultiplier * 4.5, true );
                }
                else if( simulationCase == 1 )
                {
                    terminationSettings = std::make_shared< PropagationDependentVariableTerminationSettings >(
                                dependentVariables.at( 0 ), 8.7E6, false, true,
                                tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 100 ) );
                }
                else if( simulationCase == 2 )
                {
                    std::vector< std::shared_ptr< PropagationTerminationSettings > > terminationSettingsList;
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationTimeTerminationSettings >( finalTestTime, true ) );
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationDependentVariableTerminationSettings >(
                                    dependentVariables.at( 0 ), 8.7E6, false, true,
                                    tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 100 ) ) );
                    terminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                                terminationSettingsList, true );
                }
                else if( simulationCase == 3 )
                {
                    std::vector< std::shared_ptr< PropagationTerminationSettings > > terminationSettingsList;
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationTimeTerminationSettings >( finalTestTime, true ) );
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationDependentVariableTerminationSettings >(
                                    dependentVariables.at( 0 ), 8.7E6, false, true,
                                        tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 100 ) ) );

                    terminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                                terminationSettingsList, false );
                }
                else if( simulationCase == 4 )
                {
                    std::vector< std::shared_ptr< PropagationTerminationSettings > > terminationSettingsList;
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationTimeTerminationSettings >( secondFinalTestTime, true ) );
                    terminationSettingsList.push_back(
                                std::make_shared< PropagationDependentVariableTerminationSettings >(
                                    dependentVariables.at( 0 ), 8.7E6, false, true,
                                        tudat::root_finders::bisectionRootFinderSettings( 1.0E-6, TUDAT_NAN, TUDAT_NAN, 100 ) ) );
                    terminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                                terminationSettingsList, false );
                }

                std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                        std::make_shared< TranslationalStatePropagatorSettings< double > >
                        ( centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, terminationSettings, cowell,
                          std::make_shared< DependentVariableSaveSettings >( dependentVariables ) );

                // Define integrator settings.
                const double fixedStepSize = 5.0;
                std::shared_ptr< IntegratorSettings< > > integratorSettings;
                if( integratorCase == 0 )
                {
                    integratorSettings = std::make_shared< IntegratorSettings< > >
                            ( rungeKutta4, simulationStartEpoch, directionMultiplier * fixedStepSize );
                }
                else
                {
                    integratorSettings = std::make_shared< RungeKuttaVariableStepSizeSettings< double > >
                            ( simulationStartEpoch, directionMultiplier * fixedStepSize,
                              RungeKuttaCoefficients::CoefficientSets::rungeKuttaFehlberg45,
                              1.0E-3, 1.0E3, 1.0E-12, 1.0E-12 );
                }

                // Propagate orbit with Cowell method
                SingleArcDynamicsSimulator< double > dynamicsSimulator(
                            bodies, integratorSettings, propagatorSettings, true, false, false );
                std::map< double, Eigen::VectorXd > stateHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
                std::map< double, Eigen::VectorXd > dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( );

                // Sanity check: altitude limit not violated on first step
                BOOST_CHECK_EQUAL( ( vehicleInitialState.segment( 0, 3 ).norm( ) - 8.7E6 ) < 100.0, true );

                if( simulationCase == 0 )
                {
                    // Check if propagation terminated exactly on final time
                    if( direction == 0 )
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.rbegin( )->first -
                                                      ( simulationEndEpoch - 4.5 ) ), 1.0E-10 );
                    }
                    else
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.begin( )->first -
                                                      ( simulationEndEpoch + 4.5 ) ), 1.0E-10 );
                    }
                }
                else if( simulationCase == 1 )
                {
                    // Check if propagation terminated exactly on final altitude
                    if( direction == 0 )
                    {
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ), 0.01 );
                    }
                    else
                    {
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6 ), 0.01 );
                    }
                }
                else if( simulationCase == 2 )
                {
                    // Check if propagation terminated exactly on final altitude  or final time (whichever came first)
                    // Determine by inspection: for both forward propagation, altitude condition reached first; for
                    // backward propagation, time condition reaced first
                    if( direction == 0 )
                    {
                        // Check if termination on final altitude
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ), 0.01 );

                        // Check if final time indeed not yet reached
                        BOOST_CHECK_EQUAL( ( stateHistory.rbegin( )->first - finalTestTime ) < 1.0, true );
                    }
                    else
                    {
                        // Check if termination on final time
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.begin( )->first - finalTestTime ), 1.0E-10 );

                        // Check if final altitude indeed not yet reached
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6 ) < -100.0, true );
                    }
                }
                else if( simulationCase == 3 )
                {
                    // Check if propagation terminated exactly on final altitude  or final time (both must be attained, see
                    // comment on previous test.
                    if( direction == 0 )
                    {
                        // Check if termination on final time
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.rbegin( )->first - finalTestTime ), 0.01 );

                        // Check if final altitude indeed already exceeded
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ) > 100.0, true );
                    }
                    else
                    {
                        // Check if termination on final altitude
                        BOOST_CHECK_SMALL( std::fabs( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6  ), 0.01 );

                        // Check if final time indeed already exceeded
                        BOOST_CHECK_EQUAL( ( stateHistory.begin( )->first - finalTestTime ) < -0.1, true );
                    }
                }
                else if( simulationCase == 4 )
                {
                    // Check if propagation terminated on final time
                    if( direction == 0 )
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.rbegin( )->first - secondFinalTestTime ), 0.01 );
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.rbegin( )->second( 0 ) - 8.7E6 ) > 100.0, true );
                    }
                    else
                    {
                        BOOST_CHECK_SMALL( std::fabs( stateHistory.begin( )->first - secondFinalTestTime ), 0.01 );
                        BOOST_CHECK_EQUAL( ( dependentVariableHistory.begin( )->second( 0 ) - 8.7E6 ) > 100.0, true );
                    }
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#define BOOST_TEST_MODULE IFOPT
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <ifopt_core/problem.h>
#include "ifopt/test_vars_constr_cost.h"
#include "iostream"

using namespace ifopt;
using namespace std;

BOOST_AUTO_TEST_CASE(variable)
{
  Problem nlp;
  nlp.AddVariableSet(std::make_shared<ExVariables>("var_set0"));
  nlp.AddVariableSet(std::make_shared<ExVariables>("var_set1"));

  cout << "check varialbe number" << endl;
  BOOST_CHECK_EQUAL(nlp.GetNumberOfOptimizationVariables(), 4);
}

BOOST_AUTO_TEST_CASE(GetBoundsOnOptimizationVariables)
{
  Problem nlp;
  nlp.AddVariableSet(std::make_shared<ExVariables>("var_set0"));
  nlp.AddVariableSet(std::make_shared<ExVariables>("var_set1"));

  auto bounds = nlp.GetBoundsOnOptimizationVariables();

  cout << "check bound size" << endl;

  BOOST_CHECK_EQUAL(2+2, bounds.size());

  // var_set0
  BOOST_CHECK_EQUAL(-1.0, bounds.at(0).lower_);
  BOOST_CHECK_EQUAL(+1.0, bounds.at(0).upper_);
  BOOST_CHECK_EQUAL(-inf, bounds.at(1).lower_);
  BOOST_CHECK_EQUAL(+inf, bounds.at(1).upper_);

  // var_set1
  BOOST_CHECK_EQUAL(-1.0, bounds.at(2).lower_);
  BOOST_CHECK_EQUAL(+1.0, bounds.at(2).upper_);
  BOOST_CHECK_EQUAL(-inf, bounds.at(3).lower_);
  BOOST_CHECK_EQUAL(+inf, bounds.at(3).upper_);
}

BOOST_AUTO_TEST_CASE(GetVariableValues)
{

  cout << "check GetVariableValues" << endl;
 auto var_set0 = std::make_shared<ExVariables>("var_set0");
  var_set0->SetVariables(Eigen::Vector2d(0.1, 0.2));

  auto var_set1 = std::make_shared<ExVariables>("var_set1");
  var_set1->SetVariables(Eigen::Vector2d(0.3, 0.4));

  Problem nlp;
  nlp.AddVariableSet(var_set0);
  nlp.AddVariableSet(var_set1);

  Eigen::VectorXd x = nlp.GetVariableValues();
  BOOST_CHECK_EQUAL(0.1, x(0));
  BOOST_CHECK_EQUAL(0.2, x(1));
  BOOST_CHECK_EQUAL(0.3, x(2));
  BOOST_CHECK_EQUAL(0.4, x(3));
}
BOOST_AUTO_TEST_CASE(GetNumberOfConstraints)
{

  cout << "check GetNumberOfConstraints" << endl;
  Problem nlp;
  nlp.AddConstraintSet(std::make_shared<ExConstraint>("constraint1"));

  // add same constraints again for testing.
  // notice how the Jacobian calculation inside ExConstraint-class remains the
  //same - the full Jacobian is stitched together accordingly.
  nlp.AddConstraintSet(std::make_shared<ExConstraint>("constraint2"));

  BOOST_CHECK_EQUAL(1+1, nlp.GetNumberOfConstraints());
}


#include "drake/systems/primitives/discrete_time_delay.h"

#include <cmath>
#include <limits>
#include <memory>
#include <string>

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

#include "drake/common/eigen_types.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/framework/fixed_input_port_value.h"
#include "drake/systems/framework/system_output.h"
#include "drake/systems/framework/test_utilities/scalar_conversion.h"
#include "drake/systems/primitives/affine_system.h"
#include "drake/systems/primitives/sine.h"

namespace drake {
namespace systems {
namespace {

// Define a very tight tolerance with just enough room for some roundoff.
const double kMachineTol = 10 * std::numeric_limits<double>::epsilon();

const double kUpdatePeriod = 0.1;
const int kBuffer = 5;  // Number of timesteps the signal is delayed.
const int kLength = 3;  // Length of vector passing through the block.

// A simple type containing a vector, to simplify checking expected values for
// a vector-valued DiscreteTimeDelay (`BasicVector`) and an abstract-valued
// DiscreteTimeDelay (`Value<SimpleAbstractType>`).
class SimpleAbstractType {
 public:
  explicit SimpleAbstractType(const Eigen::Vector3d& value)
      : value_(value) {}
  const Eigen::Vector3d& value() const { return value_; }
 private:
  Eigen::Vector3d value_;
};

Eigen::VectorXd ConcatenateAbstractBufferToVector(Context<double>* context) {
  // Preallocate a properly sized vector so it can be overwritten one buffer
  // entry at a time without memory issues.
  Eigen::VectorXd value =
      Eigen::VectorXd::Constant(kLength * (kBuffer + 1), -1);
  const int& oldest_index =
      context->template get_abstract_state<int>(kBuffer + 1);
  for (int ii = 0; ii < kBuffer + 1; ii++) {
    const SimpleAbstractType& next_value =
        context->get_abstract_state<SimpleAbstractType>((oldest_index + ii) %
                                                        (kBuffer + 1));
    value.segment(ii * kLength, kLength) = next_value.value();
  }
  return value;
}

class DiscreteTimeDelayTest : public ::testing::TestWithParam<bool> {
 protected:
  DiscreteTimeDelayTest() : is_abstract_(GetParam()) {}
  void SetUp() override {
    state_value_override_ << 1.0, 3.14, 2.18;
    input_value_ << 1.0, 1.0, 3.0;

    if (!is_abstract_) {
      delay_ = std::make_unique<DiscreteTimeDelay<double>>(
          kUpdatePeriod, kBuffer, kLength);
    } else {
      delay_ = std::make_unique<DiscreteTimeDelay<double>>(
          kUpdatePeriod, kBuffer,
          Value<SimpleAbstractType>(Eigen::Vector3d::Zero()));
    }
    context_ = delay_->CreateDefaultContext();
    if (!is_abstract_) {
      delay_->get_input_port().FixValue(context_.get(), input_value_);
    } else {
      delay_->get_input_port().FixValue(context_.get(),
                                        SimpleAbstractType(input_value_));
    }
  }

  std::unique_ptr<DiscreteTimeDelay<double>> delay_;
  std::unique_ptr<Context<double>> context_;

  const bool is_abstract_{};
  Eigen::Vector3d state_value_override_;
  Eigen::Vector3d input_value_;
};

// Tests that the time delay has one input, one output and no direct
// feedthrough.
TEST_P(DiscreteTimeDelayTest, Topology) {
  EXPECT_EQ(1, delay_->num_input_ports());
  EXPECT_EQ(1, context_->num_input_ports());

  EXPECT_EQ(1, delay_->num_output_ports());

  EXPECT_FALSE(delay_->HasAnyDirectFeedthrough());
}

// Tests that the time delay has correct type of state with the desired initial
// value.
TEST_P(DiscreteTimeDelayTest, InitialState) {
  const Eigen::VectorXd value_expected =
      Eigen::VectorXd::Zero(kLength * (kBuffer + 1));
  Eigen::VectorXd value;
  if (!is_abstract_) {
    const BasicVector<double>& xd = context_->get_discrete_state(0);
    EXPECT_EQ(kLength * (kBuffer + 1), xd.size());
    value = xd.CopyToVector();
  } else {
    value = ConcatenateAbstractBufferToVector(context_.get());
  }
  EXPECT_EQ(value_expected, value);
}

// Tests that the output is the delayed input.
TEST_P(DiscreteTimeDelayTest, Output) {
  const Eigen::Vector3d output_expected = state_value_override_;
  Eigen::Vector3d output;
  if (!is_abstract_) {
    BasicVector<double>& xd = context_->get_mutable_discrete_state(0);
    xd.get_mutable_value().head(kLength) = output_expected;
    output = delay_->get_output_port().Eval(*context_);
  } else {
    const int& oldest_index =
        context_->template get_abstract_state<int>(kBuffer + 1);
    SimpleAbstractType& next_value =
        context_->get_mutable_abstract_state<SimpleAbstractType>(oldest_index);
    next_value = SimpleAbstractType(state_value_override_);
    output = delay_->get_output_port()
                 .template Eval<SimpleAbstractType>(*context_)
                 .value();
  }
  EXPECT_EQ(output_expected, output);
}

// Tests that SaveInputToBuffer updates the state.
TEST_P(DiscreteTimeDelayTest, Update) {
  // Emulate an update event.
  delay_->SaveInputToBuffer(context_.get());
  Eigen::VectorXd value_expected =
      Eigen::VectorXd::Zero(kLength * (kBuffer + 1));
  value_expected.tail(kLength) = input_value_;
  Eigen::VectorXd value;
  // Check that the state has been updated to the input.
  if (!is_abstract_) {
    const BasicVector<double>& xd = context_->get_discrete_state(0);
    value = xd.CopyToVector();
  } else {
    value = ConcatenateAbstractBufferToVector(context_.get());
  }
  EXPECT_EQ(value_expected, value);
}

TEST_P(DiscreteTimeDelayTest, ToAutoDiff) {
  EXPECT_TRUE(is_autodiffxd_convertible(*delay_));
}

TEST_P(DiscreteTimeDelayTest, ToSymbolic) {
  EXPECT_TRUE(is_symbolic_convertible(*delay_));
}

// Instantiate parameterized test cases for is_abstract_ = {false, true}
INSTANTIATE_TEST_SUITE_P(test, DiscreteTimeDelayTest,
                        ::testing::Values(false, true));

// Create a simple Diagram to simulate:
//    +----------------------------------------------------------+
//    |                                                          |
//    |  +---------------+              +-----------+            |
//    |  |               |              |           |            |
//    |  |               | y=x        u |           | delayed_u  |
//    |  |    Counter    +-------------->   Delay   +----------------->
//    |  |               |              |           |            |
//    |  | xₖ₊₁ = xₖ + 1 |              |     x     |            |
//    |  +---------------+              +-----------+            |
//    |                                                          |
//    +----------------------------------------------------------+
// Then simulate for various intervals and make sure DiscreteTimeDelay sampled
// its discrete input at the expected times and returns the properly delayed
// signal.
GTEST_TEST(DiscreteTimeDelayTest, DiscreteSimulation) {
  DiagramBuilder<double> builder;
  // x[k+1] = x[k] + 1.  y[k] = x[k].  x[0] = 1.  (No inputs).
  auto counter = builder.AddSystem<AffineSystem<double>>(
      Vector1d::Constant(1.0),      // A.
      Eigen::MatrixXd::Zero(1, 0),  // B.
      Vector1d::Constant(1.0),      // f0.
      Vector1d::Constant(1.0),      // C.
      Eigen::MatrixXd::Zero(1, 0),  // D.
      Vector1d::Constant(0.0),      // y0.
      kUpdatePeriod / 2);
  counter->set_name("counter");

  auto delay =
      builder.AddSystem<DiscreteTimeDelay<double>>(kUpdatePeriod, kBuffer, 1);
  delay->set_name("delay");

  builder.Connect(counter->get_output_port(), delay->get_input_port());
  builder.ExportOutput(delay->get_output_port(), "delay_output");
  auto diagram = builder.Build();

  Simulator<double> simulator(*diagram);
  Context<double>& context = simulator.get_mutable_context();
  context.get_mutable_discrete_state(0).SetAtIndex(0, 1.0);

  auto count_eval = [&](double t) {
    return std::floor(2 * (t + kMachineTol) / kUpdatePeriod) + 1;
  };

  auto eval = [&]() { return diagram->get_output_port(0).Eval(context)[0]; };

  simulator.Initialize();
  EXPECT_EQ(0., eval());  // No update should have occurred yet.

  simulator.AdvancePendingEvents();  // Force an update at 0.
  EXPECT_EQ(0, eval());  // Should output initial value.

  // Should output initial value until delay has passed.
  simulator.AdvanceTo(0.49);
  EXPECT_EQ(0, eval());

  // Should start outputting delayed input.
  simulator.AdvanceTo(0.5);
  simulator.AdvancePendingEvents();
  EXPECT_NEAR(count_eval(0), eval(), kMachineTol);

  // Last sample at 0.9, will output value from 0.4.
  simulator.AdvanceTo(0.91);
  EXPECT_NEAR(count_eval(0.4), eval(), kMachineTol);
}

// Create a simple Diagram to simulate:
//    +-----------------------------------------------------------+
//    |                                                           |
//    |  +------------+                  +-----------+            |
//    |  |            |                  |           |            |
//    |  |            | y=a sin(wt+p)  u |           | delayed_u  |
//    |  |    sine    +------------------>   Delay   +----------------->
//    |  |            |                  |           |            |
//    |  |            |                  |     x     |            |
//    |  +------------+                  +-----------+            |
//    |                                                           |
//    +-----------------------------------------------------------+
// Then simulate for various intervals and make sure DiscreteTimeDelay sampled
// its continuous input at the expected times and returns the properly delayed
// signal.
GTEST_TEST(DiscreteTimeDelayTest, ContinuousSimulation) {
  const double sine_amplitude = 10.;
  const double sine_frequency = 2. * M_PI;
  const double sine_phase = M_PI / 4;
  const int size = 1;

  DiagramBuilder<double> builder;
  auto sine = builder.AddSystem<Sine<double>>(sine_amplitude, sine_frequency,
                                              sine_phase, size);
  sine->set_name("sine");

  auto delay = builder.AddSystem<DiscreteTimeDelay<double>>(kUpdatePeriod,
                                                            kBuffer, size);
  delay->set_name("delay");

  builder.Connect(sine->get_output_port(0), delay->get_input_port());
  builder.ExportOutput(delay->get_output_port(), "delay_output");
  auto diagram = builder.Build();

  Simulator<double> simulator(*diagram);
  const Context<double>& context = simulator.get_context();

  auto sine_eval = [&](double t) {
    return sine_amplitude * std::sin(sine_frequency * t + sine_phase);
  };

  auto eval = [&]() { return diagram->get_output_port(0).Eval(context)[0]; };

  simulator.Initialize();
  EXPECT_EQ(0., eval());  // No update should have occurred yet.

  simulator.AdvancePendingEvents();  // Force an update at 0.
  EXPECT_EQ(0, eval());  // Should output initial value.

  // Should output initial value until delay has passed.
  simulator.AdvanceTo(0.49);
  EXPECT_EQ(0, eval());

  // Should start outputting delayed input.
  simulator.AdvanceTo(0.5);
  simulator.AdvancePendingEvents();
  EXPECT_NEAR(sine_eval(0), eval(), kMachineTol);

  // Last sample at 0.9, will output value from 0.4.
  simulator.AdvanceTo(0.91);
  EXPECT_NEAR(sine_eval(0.4), eval(), kMachineTol);
}

class SymbolicDiscreteTimeDelayTest : public ::testing::Test {
 protected:
  void SetUp() override {
    const int size = 1;
    delay_ = std::make_unique<DiscreteTimeDelay<symbolic::Expression>>(
        kUpdatePeriod, kBuffer, size);

    // Initialize the context with symbolic variables.
    context_ = delay_->CreateDefaultContext();
    delay_->get_input_port().FixValue(
        context_.get(), symbolic::Expression(symbolic::Variable("u0")));
    auto& xd = context_->get_mutable_discrete_state(0);
    for (int ii = 0; ii < (kBuffer + 1); ii++) {
      xd[ii] = symbolic::Variable("x" + std::to_string(ii));
    }
  }

  std::unique_ptr<DiscreteTimeDelay<symbolic::Expression>> delay_;
  std::unique_ptr<Context<symbolic::Expression>> context_;
};

// Test that the oldest (first) entry of the buffer is outputted.
TEST_F(SymbolicDiscreteTimeDelayTest, Output) {
  const auto& out = delay_->get_output_port().Eval(*context_);
  EXPECT_EQ("x0", out[0].to_string());
}

// Tests that SaveInputVectorToBuffer updates the state (i.e. slides the buffer
// forward, adding the input value to the end of the buffer).
TEST_F(SymbolicDiscreteTimeDelayTest, Update) {
  const auto& xd = context_->get_discrete_state(0);
  for (int ii = 0; ii < (kBuffer + 1); ii++) {
    EXPECT_EQ("x" + std::to_string(ii), xd[ii].to_string());
  }
  delay_->SaveInputToBuffer(context_.get());
  for (int ii = 0; ii < kBuffer; ii++) {
    EXPECT_EQ("x" + std::to_string(ii + 1), xd[ii].to_string());
  }
  EXPECT_EQ("u0", xd[kBuffer].to_string());
}

}  // namespace
}  // namespace systems
}  // namespace drake


/*=========================================================================
 *
 *  Copyright David Doria 2011 daviddoria@gmail.com
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

// Pixel descriptors
#include "PixelDescriptors/ImagePatchPixelDescriptor.h"
#include "PixelDescriptors/FeatureVectorPixelDescriptor.h"

// Nearest neighbors
#include "NearestNeighbor/ThreeStepNearestNeighbor.hpp"
#include "NearestNeighbor/LinearSearchBestProperty.hpp"
#include "NearestNeighbor/LinearSearchKNNProperty.hpp"

// Differences
#include "DifferenceFunctions/FeatureVectorDifference.hpp"

// Boost
#include <boost/graph/grid_graph.hpp>
#include <boost/property_map/property_map.hpp>

int main(int argc, char *argv[])
{
  // Create the graph
  typedef boost::grid_graph<2> VertexListGraphType;
  const unsigned int graphSideLength = 10;
  boost::array<std::size_t, 2> graphSideLengths = { { graphSideLength, graphSideLength } };
  VertexListGraphType graph(graphSideLengths);
  typedef boost::graph_traits<VertexListGraphType>::vertex_descriptor VertexDescriptorType;

  // Get the index map
  typedef boost::property_map<VertexListGraphType, boost::vertex_index_t>::const_type IndexMapType;
  IndexMapType indexMap(get(boost::vertex_index, graph));

  // Create the descriptor maps.
  typedef boost::vector_property_map<FeatureVectorPixelDescriptor, IndexMapType> DescriptorMapType1;
  DescriptorMapType1 descriptorMap1(num_vertices(graph), indexMap);

  typedef boost::vector_property_map<FeatureVectorPixelDescriptor, IndexMapType> DescriptorMapType2;
  DescriptorMapType2 descriptorMap2(num_vertices(graph), indexMap);

  typedef boost::vector_property_map<FeatureVectorPixelDescriptor, IndexMapType> DescriptorMapType3;
  DescriptorMapType3 descriptorMap3(num_vertices(graph), indexMap);

  // Create the nearest neighbor finders
  typedef LinearSearchKNNProperty<DescriptorMapType1, FeatureVectorDifference> KNNSearchType1;
  KNNSearchType1 linearSearchKNN1(descriptorMap1, 1000);

  typedef LinearSearchKNNProperty<DescriptorMapType2, FeatureVectorDifference> KNNSearchType2;
  KNNSearchType2 linearSearchKNN2(descriptorMap2, 1000);

  typedef LinearSearchBestProperty<DescriptorMapType3, FeatureVectorDifference> BestSearchType;
  BestSearchType linearSearchBest(descriptorMap3);

  ThreeStepNearestNeighbor<KNNSearchType1, KNNSearchType2, BestSearchType> twoStepNearestNeighbor(linearSearchKNN1, linearSearchKNN2, linearSearchBest);

  return EXIT_SUCCESS;
}


#include <boost/test/unit_test.hpp>

#include "admm.hpp"

#include <iostream>

using namespace deconvolution;

BOOST_AUTO_TEST_SUITE(AdmmTests)
    BOOST_AUTO_TEST_CASE(Step1) {
        auto ep = TruncatedL1{1.0, 1.0};
        auto R = GridRangeRegularizer<1, TruncatedL1>{std::vector<int>{2}, 2, 1, ep, 1.0};

        DeconvolveParams params;
        real_1d_array hessianDiag;
        hessianDiag.setlength(16);

        minlbfgsstate lbfgsState;

        double mu1[] = {
            0.0, 1.0, 1.0, 0.0,
        };
        double nu[] = {
            0, 0, 0, 0,
        };

        auto admm = AdmmRegularizerLbfgs<1>{2, &R, &params, &hessianDiag,
            &lbfgsState, mu1, nu};

        double lambda[] = {
            0, 0, 0, 0,
        };
        double grad[] = {
            0, 0, 0, 0,
        };

        // Checking unaries
        double obj = 0;
        for (int i = 0; i < 2; ++i)
            obj += admm._evaluateUnary(i, lambda, grad, hessianDiag.getcontent());

        {
            double expectedGrad[] = {
                0.0, 1.0, 1.0, 0.0
            };

            for (int idx = 0; idx < 4; ++idx) {
                BOOST_CHECK_CLOSE(grad[idx], expectedGrad[idx], 1.0e-6);
            }
            BOOST_CHECK_CLOSE(obj, 0.0, 1.0e-6);
        }

        // Checking regularizer
        obj += R.evaluate(0, lambda, 0.01, grad, hessianDiag.getcontent());

        {
            double expectedGrad[] = {
                -0.5, 0.5, 0.5, -0.5
            };

            for (int idx = 0; idx < 4; ++idx) {
                BOOST_CHECK_CLOSE(grad[idx], expectedGrad[idx], 1.0e-6);
            }
            BOOST_CHECK_CLOSE(obj, -0.01*log(2), 1.0e-6);
        }

    }

    BOOST_AUTO_TEST_CASE(Step2) {
        auto ep = TruncatedL1{1.0, 1.0};
        auto R = GridRangeRegularizer<1, TruncatedL1>{std::vector<int>{2}, 2, 1, ep, 1.0};

        DeconvolveParams params;
        real_1d_array hessianDiag;
        hessianDiag.setlength(16);

        minlbfgsstate lbfgsState;

        double mu1[] = {
            0.0, 1.0, 1.0, 0.0,
        };
        double nu[] = {
            0, 0, 0, 0,
        };

        auto admm = AdmmRegularizerLbfgs<1>{2, &R, &params, &hessianDiag,
            &lbfgsState, mu1, nu};

        double lambda[] = {
            -0.5, 0.5, 0.5, -0.5,
        };
        double grad[] = {
            0, 0, 0, 0,
        };

        // Checking unaries
        double obj = 0;

        {
            obj += admm._evaluateUnary(0, lambda, grad, hessianDiag.getcontent());
            BOOST_CHECK_CLOSE(obj, 0.5, 1.0e-6);
            obj += admm._evaluateUnary(1, lambda, grad, hessianDiag.getcontent());
            BOOST_CHECK_CLOSE(obj, 1.0, 1.0e-6);

            double expectedGrad[] = {
                0.0, 1.0, 1.0, 0.0
            };

            for (int idx = 0; idx < 4; ++idx) {
                BOOST_CHECK_CLOSE(grad[idx], expectedGrad[idx], 1.0e-6);
            }
            BOOST_CHECK_CLOSE(obj, 1.0, 1.0e-6);
        }

        // Checking regularizer
        const double t = 0.0001;
        obj += R.evaluate(0, lambda, t, grad, hessianDiag.getcontent());

        {
            const double third = 0.333333333333333333;
            double expectedGrad[] = {
                -third, third, third, -third
            };

            for (int idx = 0; idx < 4; ++idx) {
                BOOST_CHECK_CLOSE(grad[idx], expectedGrad[idx], 1.0e-6);
            }
            BOOST_CHECK_CLOSE(obj, 1.0-t*log(3+exp(-2.0/t)), 1.0e-6);
        }

    }

    BOOST_AUTO_TEST_CASE(FullRun) {
        auto ep = TruncatedL1{1.0, 1.0};
        auto R = GridRangeRegularizer<1, TruncatedL1>{std::vector<int>{2}, 2, 1, ep, 1.0};

        DeconvolveParams params;
        real_1d_array hessianDiag;
        hessianDiag.setlength(16);

        minlbfgsstate lbfgsState;

        double mu1[] = {
            0.0, 1.0, 1.0, 0.0,
        };
        double nu[] = {
            0, 0, 0, 0,
        };

        auto admm = AdmmRegularizerLbfgs<1>{2, &R, &params, &hessianDiag,
            &lbfgsState, mu1, nu};

        double lambda[] = {
            0, 0, 0, 0,
        };

        real_1d_array lbfgsX;
        lbfgsX.setcontent(4, lambda);

        minlbfgsreport lbfgsReport;

        minlbfgscreate(1, lbfgsX, lbfgsState);
        minlbfgssetxrep(lbfgsState, true);

        minlbfgssetcond(lbfgsState, 0.001, 0.0, 0, params.maxIterations);
        minlbfgsrestartfrom(lbfgsState, lbfgsX);
        minlbfgsoptimize(lbfgsState, AdmmRegularizerLbfgs<1>::evaluate,
                AdmmRegularizerLbfgs<1>::progress, &admm);
        minlbfgsresults(lbfgsState, lbfgsX, lbfgsReport);
        BOOST_CHECK_EQUAL(lbfgsReport.terminationtype, 4);

        std::vector<double> expectedX = { -1.5, 1.5, 1.5, -1.5 };

        for (int i = 0; i < 4; ++i)
            BOOST_CHECK_CLOSE(lbfgsX[i], expectedX[i], 1.0e-6);

    }

BOOST_AUTO_TEST_SUITE_END()


/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */
/* ------------------------------------------------------
 *
 * @file logistic.cpp
 *
 * @brief Logistic-Regression functions
 *
 * We implement the conjugate-gradient method and the iteratively-reweighted-
 * least-squares method.
 *
 *//* ----------------------------------------------------------------------- */
#include <limits>
#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>
#include <modules/prob/boost.hpp>
#include <boost/math/distributions.hpp>
#include <modules/prob/student.hpp>
#include "simple_logistic.hpp"

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;

namespace modules {

// Import names from other MADlib modules
using dbal::NoSolutionFoundException;

namespace hello_world {

// FIXME this enum should be accessed by all modules that may need grouping
// valid status values
enum { IN_PROCESS, COMPLETED, TERMINATED, NULL_EMPTY };

// Internal functions
AnyType stateToResult(const Allocator &inAllocator,
                      const HandleMap<const ColumnVector, TransparentHandle<double> >& inCoef,
                      const Matrix & hessian,
                      const double &logLikelihood,
                      int status,
                      const uint64_t &numRows);

// ---------------------------------------------------------------------------
//              Logistic Regression States
// ---------------------------------------------------------------------------

/**
 * @brief Inter- and intra-iteration state for conjugate-gradient method for
 *        logistic regression
 *
 * TransitionState encapsualtes the transition state during the
 * logistic-regression aggregate function. To the database, the state is
 * exposed as a single DOUBLE PRECISION array, to the C++ code it is a proper
 * object containing scalars and vectors.
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length at least 5, and all elemenets are 0.
 *
 */
template <class Handle>
class LogRegrSPTransitionState {
    template <class OtherHandle>
    friend class LogRegrSPTransitionState;

  public:
    LogRegrSPTransitionState(const AnyType &inArray)
        : mStorage(inArray.getAs<Handle>()) {

        rebind(static_cast<uint16_t>(mStorage[1]));
    }

    /**
     * @brief Convert to backend representation
     *
     * We define this function so that we can use State in the
     * argument list and as a return type.
     */
    inline operator AnyType() const {
        return mStorage;
    }

    /**
     * @brief Initialize the conjugate-gradient state.
     *
     * This function is only called for the first iteration, for the first row.
     */
    inline void initialize(const Allocator &inAllocator, uint16_t inWidthOfX) {
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
                                             dbal::DoZero, dbal::ThrowBadAlloc>(arraySize(inWidthOfX));
        rebind(inWidthOfX);
        widthOfX = inWidthOfX;
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle>
    LogRegrSPTransitionState &operator=(
        const LogRegrSPTransitionState<OtherHandle> &inOtherState) {

        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object by copying the intra-iteration
     *     fields
     */
    template <class OtherHandle>
    LogRegrSPTransitionState &operator+=(
        const LogRegrSPTransitionState<OtherHandle> &inOtherState) {

        if (mStorage.size() != inOtherState.mStorage.size() ||
            widthOfX != inOtherState.widthOfX)
            throw std::logic_error("Internal error: Incompatible transition "
                                   "states");

        numRows += inOtherState.numRows;
        gradNew += inOtherState.gradNew;
        X_transp_AX += inOtherState.X_transp_AX;
        logLikelihood += inOtherState.logLikelihood;
        // merged state should have the higher status
        // (see top of file for more on 'status' )
        status = (inOtherState.status > status) ? inOtherState.status : status;
        return *this;
    }

    /**
     * @brief Reset the inter-iteration fields.
     */
    inline void reset() {
        numRows = 0;
        X_transp_AX.fill(0);
        gradNew.fill(0);
        logLikelihood = 0;
        status = IN_PROCESS;
    }

  private:
    static inline size_t arraySize(const uint16_t inWidthOfX) {
        return 6 + inWidthOfX * inWidthOfX + 4 * inWidthOfX;
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param inWidthOfX The number of independent variables.
     *
     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: iteration (current iteration)
     * - 1: widthOfX (number of coefficients)
     * - 2: coef (vector of coefficients)
     * - 2 + widthOfX: dir (direction)
     * - 2 + 2 * widthOfX: grad (gradient)
     * - 2 + 3 * widthOfX: beta (scale factor)
     *
     * Intra-iteration components (updated in transition step):
     * - 3 + 3 * widthOfX: numRows (number of rows already processed in this iteration)
     * - 4 + 3 * widthOfX: gradNew (intermediate value for gradient)
     * - 4 + 4 * widthOfX: X_transp_AX (X^T A X)
     * - 4 + widthOfX * widthOfX + 4 * widthOfX: logLikelihood ( ln(l(c)) )
     */
    void rebind(uint16_t inWidthOfX) {
        iteration.rebind(&mStorage[0]);
        widthOfX.rebind(&mStorage[1]);
        coef.rebind(&mStorage[2], inWidthOfX);
        dir.rebind(&mStorage[2 + inWidthOfX], inWidthOfX);
        grad.rebind(&mStorage[2 + 2 * inWidthOfX], inWidthOfX);
        beta.rebind(&mStorage[2 + 3 * inWidthOfX]);
        numRows.rebind(&mStorage[3 + 3 * inWidthOfX]);
        gradNew.rebind(&mStorage[4 + 3 * inWidthOfX], inWidthOfX);
        X_transp_AX.rebind(&mStorage[4 + 4 * inWidthOfX], inWidthOfX, inWidthOfX);
        logLikelihood.rebind(&mStorage[4 + inWidthOfX * inWidthOfX + 4 * inWidthOfX]);
        status.rebind(&mStorage[5 + inWidthOfX * inWidthOfX + 4 * inWidthOfX]);
    }

    Handle mStorage;

  public:
    typename HandleTraits<Handle>::ReferenceToUInt32 iteration;
    typename HandleTraits<Handle>::ReferenceToUInt16 widthOfX;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap coef;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap dir;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap grad;
    typename HandleTraits<Handle>::ReferenceToDouble beta;

    typename HandleTraits<Handle>::ReferenceToUInt64 numRows;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap gradNew;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap X_transp_AX;
    typename HandleTraits<Handle>::ReferenceToDouble logLikelihood;
    typename HandleTraits<Handle>::ReferenceToUInt16 status;
};

/**
 * @brief Logistic function
 */
inline double sigma(double x) {
    return 1. / (1. + std::exp(-x));
}

/**
 * @brief Perform the logistic-regression transition step
 */
AnyType
logregr_simple_step_transition::run(AnyType &args) {
    LogRegrSPTransitionState<MutableArrayHandle<double> > state = args[0];
    if (args[1].isNull() || args[2].isNull()) { return args[0]; }
    double y = args[1].getAs<bool>() ? 1. : -1.;
    MappedColumnVector x;
    try {
        // an exception is raised in the backend if args[2] contains nulls
        MappedColumnVector xx = args[2].getAs<MappedColumnVector>();
        // x is a const reference, we can only rebind to change its pointer
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    // The following check was added with MADLIB-138.
    if (!dbal::eigen_integration::isfinite(x)) {
        warning("Design matrix is not finite.");
        state.status = TERMINATED;
        return state;
    }

    if (state.numRows == 0) {
        if (x.size() > std::numeric_limits<uint16_t>::max()){
            warning("Number of independent variables cannot be larger than 65535.");
            state.status = TERMINATED;
            return state;
        }

        state.initialize(*this, static_cast<uint16_t>(x.size()));
        if (!args[3].isNull()) {
            LogRegrSPTransitionState<ArrayHandle<double> > previousState = args[3];

            state = previousState;
            state.reset();
        }
    }
    // Now do the transition step
    state.numRows++;
    double xc = dot(x, state.coef);
    state.gradNew.noalias() += sigma(-y * xc) * y * trans(x);

    // Note: sigma(-x) = 1 - sigma(x).
    double a = sigma(xc) * sigma(-xc);
    state.X_transp_AX += x * trans(x) * a;

    state.logLikelihood -= std::log( 1. + std::exp(-y * xc) );

    return state;
}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
logregr_simple_step_merge_states::run(AnyType &args) {
    LogRegrSPTransitionState<MutableArrayHandle<double> > stateLeft = args[0];
    LogRegrSPTransitionState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numRows == 0)
        return stateRight;
    else if (stateRight.numRows == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

/**
 * @brief Perform the logistic-regression final step
 */
AnyType
logregr_simple_step_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    LogRegrSPTransitionState<MutableArrayHandle<double> > state = args[0];

    // Aggregates that haven't seen any data just return Null.
    if (state.numRows == 0){
        state.status = NULL_EMPTY;
        return state;
    }

    // Note: k = state.iteration
    if (state.iteration == 0) {
        // Iteration computes the gradient

        state.dir = state.gradNew;
        state.grad = state.gradNew;
    } else {
        // We use the Hestenes-Stiefel update formula:
        //
        //            g_k^T (g_k - g_{k-1})
        // beta_k = -------------------------
        //          d_{k-1}^T (g_k - g_{k-1})
        ColumnVector gradNewMinusGrad = state.gradNew - state.grad;
        state.beta
            = dot(state.gradNew, gradNewMinusGrad)
            / dot(state.dir, gradNewMinusGrad);

        // Do a direction restart (Powell restart)
        // Note: This is testing whether state.beta < 0 if state.beta were
        // assigned according to Polak-Ribière
        if (dot(state.gradNew, gradNewMinusGrad)
            / dot(state.grad, state.grad) <= std::numeric_limits<double>::denorm_min()) state.beta = 0;

        // d_k = g_k - beta_k * d_{k-1}
        state.dir = state.gradNew - state.beta * state.dir;
        state.grad = state.gradNew;
    }

    // H_k = - X^T A_k X
    // where A_k = diag(a_1, ..., a_n) and a_i = sigma(x_i c_{k-1}) sigma(-x_i c_{k-1})
    //
    //             g_k^T d_k
    // alpha_k = -------------
    //           d_k^T H_k d_k
    //
    // c_k = c_{k-1} - alpha_k * d_k
    state.coef += dot(state.grad, state.dir) /
        as_scalar(trans(state.dir) * state.X_transp_AX * state.dir)
        * state.dir;

    if(!state.coef.is_finite()){
        // we don't throw an error - instead set the state status to allow
        // other states (possibly trained as part of group by) to continue
        // training
        warning("Over- or underflow in conjugate-gradient step, while updating "
              "coefficients. Input data is likely of poor numerical condition.");
        state.status = TERMINATED;
        return state;
    }

    state.iteration++;
    return state;
}

/**
 * @brief Return the difference in log-likelihood between two states
 */
AnyType
internal_logregr_simple_step_distance::run(AnyType &args) {
    LogRegrSPTransitionState<ArrayHandle<double> > stateLeft = args[0];
    LogRegrSPTransitionState<ArrayHandle<double> > stateRight = args[1];

    if(stateLeft.status == NULL_EMPTY || stateRight.status == NULL_EMPTY){
        return 0.0;
    }
    return std::abs(stateLeft.logLikelihood - stateRight.logLikelihood);
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
internal_logregr_simple_result::run(AnyType &args) {
    LogRegrSPTransitionState<ArrayHandle<double> > state = args[0];
    if (state.status == NULL_EMPTY)
        return Null();

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        state.X_transp_AX, EigenvaluesOnly, ComputePseudoInverse);

    return stateToResult(*this, state.coef,
                         state.X_transp_AX, state.logLikelihood,
                         state.status, state.numRows);
}

/**
 * @brief Compute the diagnostic statistics
 *
 * This function wraps the common parts of computing the results for both the
 * CG and the IRLS method.
 */
AnyType stateToResult(
    const Allocator &inAllocator,
    const HandleMap<const ColumnVector, TransparentHandle<double> > &inCoef,
    const Matrix & hessian,
    const double &logLikelihood,
    int status,
    const uint64_t &numRows) {

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        hessian, EigenvaluesOnly, ComputePseudoInverse);

    const Matrix &inverse_of_X_transp_AX = decomposition.pseudoInverse();
    const ColumnVector &diagonal_of_X_transp_AX = inverse_of_X_transp_AX.diagonal();

    MutableNativeColumnVector stdErr(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldZStats(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldPValues(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector oddsRatios(
        inAllocator.allocateArray<double>(inCoef.size()));

    for (Index i = 0; i < inCoef.size(); ++i) {
        stdErr(i) = std::sqrt(diagonal_of_X_transp_AX(i));
        waldZStats(i) = inCoef(i) / stdErr(i);
        waldPValues(i) = 2. * prob::cdf( prob::normal(),
                                         -std::abs(waldZStats(i)));
        oddsRatios(i) = std::exp( inCoef(i) );
    }

    // Return all coefficients, standard errors, etc. in a tuple
    AnyType tuple;
    tuple << inCoef << logLikelihood << stdErr << waldZStats << waldPValues
          << oddsRatios << inverse_of_X_transp_AX
          << sqrt(decomposition.conditionNo()) << status << numRows;
    return tuple;
}

} // namespace hello_world
} // namespace modules
} // namespace madlib


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_DRIVER_HPP

#include <boost/numeric/bindings/lapack/driver/gbsv.hpp>
#include <boost/numeric/bindings/lapack/driver/gbsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/gees.hpp>
#include <boost/numeric/bindings/lapack/driver/geesx.hpp>
#include <boost/numeric/bindings/lapack/driver/geev.hpp>
#include <boost/numeric/bindings/lapack/driver/geevx.hpp>
#include <boost/numeric/bindings/lapack/driver/gegv.hpp>
#include <boost/numeric/bindings/lapack/driver/gejsv.hpp>
#include <boost/numeric/bindings/lapack/driver/gels.hpp>
#include <boost/numeric/bindings/lapack/driver/gelsd.hpp>
#include <boost/numeric/bindings/lapack/driver/gelss.hpp>
#include <boost/numeric/bindings/lapack/driver/gelsy.hpp>
#include <boost/numeric/bindings/lapack/driver/gesdd.hpp>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/lapack/driver/gesvd.hpp>
#include <boost/numeric/bindings/lapack/driver/gesvx.hpp>
#include <boost/numeric/bindings/lapack/driver/gges.hpp>
#include <boost/numeric/bindings/lapack/driver/ggesx.hpp>
#include <boost/numeric/bindings/lapack/driver/ggev.hpp>
#include <boost/numeric/bindings/lapack/driver/ggevx.hpp>
#include <boost/numeric/bindings/lapack/driver/ggglm.hpp>
#include <boost/numeric/bindings/lapack/driver/gglse.hpp>
#include <boost/numeric/bindings/lapack/driver/ggsvd.hpp>
#include <boost/numeric/bindings/lapack/driver/gtsv.hpp>
#include <boost/numeric/bindings/lapack/driver/gtsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/hbev.hpp>
#include <boost/numeric/bindings/lapack/driver/hbevd.hpp>
#include <boost/numeric/bindings/lapack/driver/hbevx.hpp>
#include <boost/numeric/bindings/lapack/driver/hbgv.hpp>
#include <boost/numeric/bindings/lapack/driver/hbgvd.hpp>
#include <boost/numeric/bindings/lapack/driver/hbgvx.hpp>
#include <boost/numeric/bindings/lapack/driver/heev.hpp>
#include <boost/numeric/bindings/lapack/driver/heevd.hpp>
#include <boost/numeric/bindings/lapack/driver/heevr.hpp>
#include <boost/numeric/bindings/lapack/driver/heevx.hpp>
#include <boost/numeric/bindings/lapack/driver/hegv.hpp>
#include <boost/numeric/bindings/lapack/driver/hegvd.hpp>
#include <boost/numeric/bindings/lapack/driver/hegvx.hpp>
#include <boost/numeric/bindings/lapack/driver/hesv.hpp>
#include <boost/numeric/bindings/lapack/driver/hesvx.hpp>
#include <boost/numeric/bindings/lapack/driver/hpev.hpp>
#include <boost/numeric/bindings/lapack/driver/hpevd.hpp>
#include <boost/numeric/bindings/lapack/driver/hpevx.hpp>
#include <boost/numeric/bindings/lapack/driver/hpgv.hpp>
#include <boost/numeric/bindings/lapack/driver/hpgvd.hpp>
#include <boost/numeric/bindings/lapack/driver/hpgvx.hpp>
#include <boost/numeric/bindings/lapack/driver/hpsv.hpp>
#include <boost/numeric/bindings/lapack/driver/hpsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/iter_gesv.hpp>
#include <boost/numeric/bindings/lapack/driver/iter_posv.hpp>
#include <boost/numeric/bindings/lapack/driver/pbsv.hpp>
#include <boost/numeric/bindings/lapack/driver/pbsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/posv.hpp>
#include <boost/numeric/bindings/lapack/driver/posvx.hpp>
#include <boost/numeric/bindings/lapack/driver/ppsv.hpp>
#include <boost/numeric/bindings/lapack/driver/ppsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/ptsv.hpp>
#include <boost/numeric/bindings/lapack/driver/ptsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/sbev.hpp>
#include <boost/numeric/bindings/lapack/driver/sbevd.hpp>
#include <boost/numeric/bindings/lapack/driver/sbevx.hpp>
#include <boost/numeric/bindings/lapack/driver/sbgv.hpp>
#include <boost/numeric/bindings/lapack/driver/sbgvd.hpp>
#include <boost/numeric/bindings/lapack/driver/sbgvx.hpp>
#include <boost/numeric/bindings/lapack/driver/spev.hpp>
#include <boost/numeric/bindings/lapack/driver/spevd.hpp>
#include <boost/numeric/bindings/lapack/driver/spevx.hpp>
#include <boost/numeric/bindings/lapack/driver/spgv.hpp>
#include <boost/numeric/bindings/lapack/driver/spgvd.hpp>
#include <boost/numeric/bindings/lapack/driver/spgvx.hpp>
#include <boost/numeric/bindings/lapack/driver/spsv.hpp>
#include <boost/numeric/bindings/lapack/driver/spsvx.hpp>
#include <boost/numeric/bindings/lapack/driver/stev.hpp>
#include <boost/numeric/bindings/lapack/driver/stevd.hpp>
#include <boost/numeric/bindings/lapack/driver/stevr.hpp>
#include <boost/numeric/bindings/lapack/driver/stevx.hpp>
#include <boost/numeric/bindings/lapack/driver/syev.hpp>
#include <boost/numeric/bindings/lapack/driver/syevd.hpp>
#include <boost/numeric/bindings/lapack/driver/syevr.hpp>
#include <boost/numeric/bindings/lapack/driver/syevx.hpp>
#include <boost/numeric/bindings/lapack/driver/sygv.hpp>
#include <boost/numeric/bindings/lapack/driver/sygvd.hpp>
#include <boost/numeric/bindings/lapack/driver/sygvx.hpp>
#include <boost/numeric/bindings/lapack/driver/sysv.hpp>
#include <boost/numeric/bindings/lapack/driver/sysvx.hpp>

#endif


#ifndef HOPS_LINEARPROGRAMSOLUTION_HPP
#define HOPS_LINEARPROGRAMSOLUTION_HPP

#include <Eigen/Core>
#include <utility>
#include "LinearProgramStatus.hpp"

namespace hops {
    struct LinearProgramSolution {
        LinearProgramSolution(double objectiveValue, Eigen::VectorXd solution, LinearProgramStatus status)
                : objectiveValue(objectiveValue), optimalParameters(std::move(solution)), status(status) {}

        bool operator==(const LinearProgramSolution &rhs) const {
            return objectiveValue == rhs.objectiveValue &&
                   optimalParameters == rhs.optimalParameters &&
                   status == rhs.status;
        }

        bool operator!=(const LinearProgramSolution &rhs) const {
            return !(rhs == *this);
        }

        double objectiveValue;
        Eigen::VectorXd optimalParameters;
        LinearProgramStatus status;
    };
}

#endif //HOPS_LINEARPROGRAMSOLUTION_HPP


#include <gtest/gtest.h>
#include "test_complex_helper.hpp"
#include "pitts_qubit_simulator.hpp"
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

using namespace PITTS;

namespace
{
  constexpr auto eps = 1.e-10;

  using StateVec = FixedTensorTrain<std::complex<double>,2>;

  // helper function to convert a tensor train state vector to a plain dense vector
  Eigen::VectorXcd toDenseVector(const StateVec& TTv)
  {
    const auto nDims = TTv.nDims();
    const auto n = 1UL << nDims;
    Eigen::VectorXcd v(n);

    std::vector<int> idx_i(nDims);
    StateVec e_i(nDims);
    for(std::size_t i = 0; i < n; i++)
    {
      for(int j = 0; j < nDims; j++)
        idx_i[j] = (i & (1UL << j)) ? 1 : 0;
      e_i.setUnit(idx_i);

      v(i) = dot(e_i,TTv);
    }
    return v;
  }
}

TEST(PITTS_QubitSimulator, qubitIds_singleQubit)
{
  QubitSimulator qsim;

  // we don't have any qubits initially...
  ASSERT_THROW(qsim.isClassical(0), std::out_of_range);
  ASSERT_THROW(qsim.getClassicalValue(0), std::out_of_range);

  // allocate / deallocate
  qsim.allocateQubit(57);
  ASSERT_THROW(qsim.deallocateQubit(0), std::out_of_range);
  ASSERT_THROW(qsim.allocateQubit(57), std::invalid_argument);
  ASSERT_TRUE(qsim.isClassical(57));
  qsim.deallocateQubit(57);
  ASSERT_THROW(qsim.deallocateQubit(57), std::out_of_range);
}

TEST(PITTS_QubitSimulator, qubitIds_twoQubits)
{
  QubitSimulator qsim;

  // allocate / deallocate
  qsim.allocateQubit(57);
  qsim.allocateQubit(17);
  ASSERT_TRUE(qsim.isClassical(57));
  ASSERT_TRUE(qsim.isClassical(17));
  qsim.deallocateQubit(57);
  ASSERT_TRUE(qsim.isClassical(17));
  ASSERT_THROW(qsim.deallocateQubit(57), std::out_of_range);
  qsim.deallocateQubit(17);
}

TEST(PITTS_QubitSimulator, qubitIds_multipl)
{
  QubitSimulator qsim;

  // allocate / deallocate (with multiple swaps)
  qsim.allocateQubit(57);
  qsim.allocateQubit(17);
  qsim.allocateQubit(32);
  qsim.allocateQubit(5);
  qsim.deallocateQubit(17);
  qsim.deallocateQubit(57);
  ASSERT_THROW(qsim.allocateQubit(32), std::invalid_argument);
  ASSERT_THROW(qsim.deallocateQubit(57), std::out_of_range);
  qsim.allocateQubit(19);
  qsim.deallocateQubit(5);
  qsim.deallocateQubit(32);
  qsim.deallocateQubit(19);
}

TEST(PITTS_QubitSimulator, initializedToZero)
{
  QubitSimulator qsim;

  qsim.allocateQubit(57);
  qsim.allocateQubit(17);
  qsim.allocateQubit(32);
  qsim.allocateQubit(5);
  qsim.deallocateQubit(17);
  qsim.allocateQubit(0);

  EXPECT_EQ(false, qsim.getClassicalValue(57));
  EXPECT_EQ(false, qsim.getClassicalValue(32));
  ASSERT_THROW(qsim.getClassicalValue(17), std::out_of_range);
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(0));
}

TEST(PITTS_QubitSimulator, collapse_alreadyClassicalZero)
{
  QubitSimulator qsim;

  qsim.allocateQubit(57);
  qsim.allocateQubit(17);
  qsim.allocateQubit(32);
  qsim.allocateQubit(5);

  // full collapse
  const std::vector<bool> result = {false,false,false,false};
  qsim.collapseWavefunction({5,32,17,57}, result);

  // partial collapse
  const std::vector<bool> result2 = {false,false};
  qsim.collapseWavefunction({32,17}, result2);

  // non-existing ids
  const std::vector<bool> tmp(3);
  ASSERT_THROW(qsim.collapseWavefunction({1,2,3}, tmp), std::out_of_range);

  // incorrect dimensions
  ASSERT_THROW(qsim.collapseWavefunction({32,17}, tmp), std::invalid_argument);

  // invalid collapse (impossible values)
  const std::vector<bool> result3 = {true,false};
  ASSERT_THROW(qsim.collapseWavefunction({32,5}, result3), std::invalid_argument);
}

TEST(PITTS_QubitSimulator, getProbability_alreadyClassicalZero)
{
  QubitSimulator qsim;

  qsim.allocateQubit(57);
  qsim.allocateQubit(17);
  qsim.allocateQubit(32);
  qsim.allocateQubit(5);

  // full probability
  const std::vector<bool> result_ref = {false,false,false,false};
  EXPECT_NEAR(1., qsim.getProbability({5,32,17,57}, result_ref), eps);

  const std::vector<bool> other_result = {true,false,true,false};
  EXPECT_NEAR(0., qsim.getProbability({5,32,17,57}, other_result), eps);

  // partial probability
  const std::vector<bool> result2_ref = {false,false};
  EXPECT_NEAR(1., qsim.getProbability({32,17}, result2_ref), eps);

  const std::vector<bool> other_result2 = {true,false};
  EXPECT_NEAR(0., qsim.getProbability({32,17}, other_result2), eps);

  // non-existing ids
  ASSERT_THROW(qsim.getProbability({32,2}, {true,false}), std::out_of_range);

  // inconsistent input dimensions
  ASSERT_THROW(qsim.getProbability({32,2}, {true,false,true}), std::invalid_argument);
}

TEST(PITTS_QubitSimulator, measureQubits_alreadyClassicalZero)
{
  QubitSimulator qsim;

  qsim.allocateQubit(57);
  qsim.allocateQubit(17);
  qsim.allocateQubit(32);
  qsim.allocateQubit(5);

  // full measurement
  const auto result = qsim.measureQubits({5,32,17,57});
  const std::vector<bool> result_ref = {false,false,false,false};
  EXPECT_EQ(result_ref, result);

  // partial measurement
  const auto result2 = qsim.measureQubits({32,17});
  const std::vector<bool> result2_ref = {false,false};
  EXPECT_EQ(result2_ref, result2);

  // non-existing ids
  ASSERT_THROW(qsim.measureQubits({32,2}), std::out_of_range);
}

TEST(PITTS_QubitSimulator, applySingleQubitGate_classicalBitFlip)
{
  QubitSimulator::Matrix2 flipGate;
  flipGate[0][0] = 0;
  flipGate[0][1] = 1;
  flipGate[1][0] = 1;
  flipGate[1][1] = 0;

  QubitSimulator qsim;

  // allocate some qubits
  qsim.allocateQubit(1);
  qsim.allocateQubit(3);
  qsim.allocateQubit(5);
  qsim.allocateQubit(7);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(false, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));

  qsim.applySingleQubitGate(3, flipGate);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(true, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));

  // requires reordering of the internal state vector!
  qsim.allocateQubit(9);
  qsim.deallocateQubit(1);

  EXPECT_EQ(false, qsim.getClassicalValue(9));
  EXPECT_EQ(true, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));

  const auto result = qsim.measureQubits({9,5,7,3});
  const std::vector<bool> result_ref = {false,false,false,true};
  EXPECT_EQ(result_ref, result);

  qsim.applySingleQubitGate(7, flipGate);
  qsim.applySingleQubitGate(5, flipGate);
  qsim.applySingleQubitGate(3, flipGate);
  qsim.applySingleQubitGate(9, flipGate);

  const auto result2 = qsim.measureQubits({9,5,7,3});
  const std::vector<bool> result2_ref = {true,true,true,false};
  EXPECT_EQ(result2_ref, result2);
}

TEST(PITTS_QubitSimulator, applyTwoQubitGate_classicalBitFlip)
{
  QubitSimulator::Matrix4 flipBothGate = {};
  flipBothGate[0][3] = 1;
  flipBothGate[1][2] = 1;
  flipBothGate[2][1] = 1;
  flipBothGate[3][0] = 1;

  QubitSimulator qsim;

  // allocate some qubits
  qsim.allocateQubit(1);
  qsim.allocateQubit(3);
  qsim.allocateQubit(5);
  qsim.allocateQubit(7);
  qsim.allocateQubit(9);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(false, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  qsim.applyTwoQubitGate(3, 5, flipBothGate);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(true, qsim.getClassicalValue(3));
  EXPECT_EQ(true, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  qsim.applyTwoQubitGate(5, 7, flipBothGate);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(true, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(true, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  qsim.applyTwoQubitGate(3, 7, flipBothGate);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(false, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  qsim.applyTwoQubitGate(9, 1, flipBothGate);

  EXPECT_EQ(true, qsim.getClassicalValue(1));
  EXPECT_EQ(false, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(true, qsim.getClassicalValue(9));
}

TEST(PITTS_QubitSimulator, applySingleAndTwoQubitGates_simpleBellState)
{
  QubitSimulator::Matrix2 hadamardGate;
  hadamardGate[0][0] = 1./std::sqrt(2.);
  hadamardGate[0][1] = 1./std::sqrt(2.);
  hadamardGate[1][0] = 1./std::sqrt(2.);
  hadamardGate[1][1] = -1./std::sqrt(2.);

  QubitSimulator::Matrix4 cnotGate = {};
  cnotGate[0][0] = 1;
  cnotGate[1][1] = 1;
  cnotGate[2][3] = 1;
  cnotGate[3][2] = 1;

  QubitSimulator qsim;

  // allocate some qubits
  qsim.allocateQubit(1);
  qsim.allocateQubit(3);
  qsim.allocateQubit(5);
  qsim.allocateQubit(7);
  qsim.allocateQubit(9);

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(false, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  qsim.applySingleQubitGate(5, hadamardGate);

  EXPECT_EQ(true, qsim.isClassical(1));
  EXPECT_EQ(true, qsim.isClassical(3));
  EXPECT_EQ(false, qsim.isClassical(5));
  EXPECT_EQ(true, qsim.isClassical(7));
  EXPECT_EQ(true, qsim.isClassical(9));

  // create entangled state between 3 and 5
  qsim.applyTwoQubitGate(3, 5, cnotGate);

  EXPECT_EQ(true, qsim.isClassical(1));
  EXPECT_EQ(false, qsim.isClassical(3));
  EXPECT_EQ(false, qsim.isClassical(5));
  EXPECT_EQ(true, qsim.isClassical(7));
  EXPECT_EQ(true, qsim.isClassical(9));

  // check probabilities
  ASSERT_NEAR(0.5, qsim.getProbability({3,5}, {false,false}), eps);
  ASSERT_NEAR(0.5, qsim.getProbability({3,5}, {true,true}), eps);

  // cannot deallocate entangled state...
  ASSERT_THROW(qsim.deallocateQubit(3), std::invalid_argument);

  // collapse to classical state
  qsim.collapseWavefunction({3,5}, {false,false});

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(false, qsim.getClassicalValue(3));
  EXPECT_EQ(false, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  // check that the scaling is still correct after the collapse
  ASSERT_NEAR(1., qsim.getProbability({1,3,5,7,9}, {false,false,false,false,false}), eps);

  // same as before but try other collapse
  qsim.applySingleQubitGate(5, hadamardGate);
  qsim.applyTwoQubitGate(3, 5, cnotGate);
  ASSERT_THROW(qsim.collapseWavefunction({3,5}, {true,false}), std::invalid_argument);
  ASSERT_THROW(qsim.collapseWavefunction({3,5}, {false,true}), std::invalid_argument);
  qsim.collapseWavefunction({3,5}, {true,true});

  EXPECT_EQ(false, qsim.getClassicalValue(1));
  EXPECT_EQ(true, qsim.getClassicalValue(3));
  EXPECT_EQ(true, qsim.getClassicalValue(5));
  EXPECT_EQ(false, qsim.getClassicalValue(7));
  EXPECT_EQ(false, qsim.getClassicalValue(9));

  ASSERT_NEAR(1., qsim.getProbability({1,3,5,7,9}, {false,true,true,false,false}), eps);
}

TEST(PITTS_QubitSimulator, emulateTimeEvolution)
{
  QubitSimulator qsim;

  // allocate some qubits
  qsim.allocateQubit(1);
  qsim.allocateQubit(3);
  qsim.allocateQubit(5);
  qsim.allocateQubit(7);
  qsim.allocateQubit(9);

  // construct some arbitrary Hamiltonian
  const std::vector<QubitSimulator::QubitId> ids = {3,5,7};
  QubitSimulator::Matrix2 h3;
  h3[0][0] = 0.;
  h3[0][1] = 0.77;
  h3[1][0] = 0.77;
  h3[1][1] = 0.;

  QubitSimulator::Matrix2 h5;
  h5[0][0] = 1.55;
  h5[0][1] = 0.;
  h5[1][0] = 0.;
  h5[1][1] = -1.55;

  QubitSimulator::Matrix2 h7;
  h7[0][0] = 0.;
  h7[0][1] = std::complex<double>(0., -1.33);
  h7[1][0] = std::complex<double>(0., 1.33);
  h7[1][1] = 0.;
  const std::vector<QubitSimulator::Matrix2> terms = {h3,h5,h7};

  // compose complete Hamiltonian matrix
  using Matrix32 = Eigen::Matrix<std::complex<double>, 32, 32>;
  Matrix32 H;
  {
    StateVec col_i(5);
    std::vector<int> idx_i(5);
    for(std::size_t i = 0; i < 32; i++)
    {
      for(int j = 0; j < 5; j++)
        idx_i[j] = (i & (1UL << j)) ? 1 : 0;

      col_i.setUnit(idx_i);

      PITTS::apply(col_i.editableSubTensors()[1], h3);
      PITTS::apply(col_i.editableSubTensors()[2], h5);
      PITTS::apply(col_i.editableSubTensors()[3], h7);
      H.col(i) = toDenseVector(col_i);
    }
  }

  // H should be self-adjoint
  ASSERT_NEAR(0., (H-H.adjoint()).norm(), eps);

  // helper variable for complex number i
  static constexpr auto i = std::complex<double>(0, 1);

  // get the initial state
  auto x0 = toDenseVector(qsim.getWaveFunction());

  // no change
  qsim.emulateTimeEvolution(0., ids, terms);
  auto x = toDenseVector(qsim.getWaveFunction());
  auto x_ref = x0;
  EXPECT_NEAR(0., (x-x_ref).norm(), eps);

  // forward
  qsim.emulateTimeEvolution(0.3, ids, terms);
  x = toDenseVector(qsim.getWaveFunction());
  x_ref = (i*0.3*H).exp() * x0;
  EXPECT_NEAR(0., (x-x_ref).norm(), eps);

  // backward
  qsim.emulateTimeEvolution(-0.17, ids, terms);
  x = toDenseVector(qsim.getWaveFunction());
  x_ref = (i*0.13*H).exp() * x0;
  EXPECT_NEAR(0., (x-x_ref).norm(), eps);

  // use a non-classical state to make this more interesting!
  {
    QubitSimulator::Matrix2 phaseGate;
    phaseGate[0][0] = 1.;
    phaseGate[0][1] = 0.;
    phaseGate[1][0] = 0.;
    phaseGate[1][1] = i;

    QubitSimulator::Matrix2 hadamardGate;
    hadamardGate[0][0] = 1./std::sqrt(2.);
    hadamardGate[0][1] = 1./std::sqrt(2.);
    hadamardGate[1][0] = 1./std::sqrt(2.);
    hadamardGate[1][1] = -1./std::sqrt(2.);

    QubitSimulator::Matrix4 cnotGate = {};
    cnotGate[0][0] = 1;
    cnotGate[1][1] = 1;
    cnotGate[2][3] = 1;
    cnotGate[3][2] = 1;

    qsim.applySingleQubitGate(1, phaseGate);
    qsim.applySingleQubitGate(5, hadamardGate);
    qsim.applySingleQubitGate(9, hadamardGate);
    qsim.applySingleQubitGate(9, phaseGate);
    qsim.applyTwoQubitGate(7, 9, cnotGate);
    qsim.applyTwoQubitGate(3, 5, cnotGate);
    qsim.applyTwoQubitGate(1, 3, cnotGate);
    qsim.applySingleQubitGate(5, phaseGate);
    qsim.applyTwoQubitGate(5, 7, cnotGate);
  }

  // get the initial state
  x0 = toDenseVector(qsim.getWaveFunction());

  // no change
  qsim.emulateTimeEvolution(0., ids, terms);
  x = toDenseVector(qsim.getWaveFunction());
  x_ref = x0;
  EXPECT_NEAR(0., (x-x_ref).norm(), eps);

  // forward
  qsim.emulateTimeEvolution(0.3, ids, terms);
  x = toDenseVector(qsim.getWaveFunction());
  x_ref = (i*0.3*H).exp() * x0;
  EXPECT_NEAR(0., (x-x_ref).norm(), eps);

  // backward
  qsim.emulateTimeEvolution(-0.17, ids, terms);
  x = toDenseVector(qsim.getWaveFunction());
  x_ref = (i*0.13*H).exp() * x0;
  EXPECT_NEAR(0., (x-x_ref).norm(), eps);
}

TEST(PITTS_QubitSimulator, getExpectationValue)
{
  QubitSimulator qsim;

  // allocate some qubits
  qsim.allocateQubit(1);
  qsim.allocateQubit(3);
  qsim.allocateQubit(5);
  qsim.allocateQubit(7);
  qsim.allocateQubit(9);

  // construct some arbitrary Hamiltonian
  QubitSimulator::Matrix2 up = {};
  up[0][0] = 1.;

  QubitSimulator::Matrix2 down = {};
  down[1][1] = 1.;

  QubitSimulator::Matrix2 upDoubled = {};
  upDoubled[0][0] = 2.;

  // initially everything points up
  EXPECT_NEAR(1., qsim.getExpectationValue({1}, {up}), eps);
  EXPECT_NEAR(1., qsim.getExpectationValue({3}, {up}), eps);
  EXPECT_NEAR(1., qsim.getExpectationValue({5}, {up}), eps);
  EXPECT_NEAR(1., qsim.getExpectationValue({1,9}, {up,down}), eps);
  EXPECT_NEAR(3., qsim.getExpectationValue({3,5,7}, {up,up,up}), eps);

  // use a non-classical state to make this more interesting!
  {
    // helper variable for complex number i
    static constexpr auto i = std::complex<double>(0, 1);

    QubitSimulator::Matrix2 phaseGate;
    phaseGate[0][0] = 1.;
    phaseGate[0][1] = 0.;
    phaseGate[1][0] = 0.;
    phaseGate[1][1] = i;

    QubitSimulator::Matrix2 hadamardGate;
    hadamardGate[0][0] = 1./std::sqrt(2.);
    hadamardGate[0][1] = 1./std::sqrt(2.);
    hadamardGate[1][0] = 1./std::sqrt(2.);
    hadamardGate[1][1] = -1./std::sqrt(2.);

    QubitSimulator::Matrix4 cnotGate = {};
    cnotGate[0][0] = 1;
    cnotGate[1][1] = 1;
    cnotGate[2][3] = 1;
    cnotGate[3][2] = 1;

    qsim.applySingleQubitGate(1, phaseGate);
    qsim.applySingleQubitGate(5, hadamardGate);
    qsim.applySingleQubitGate(9, hadamardGate);
    qsim.applySingleQubitGate(9, phaseGate);
    qsim.applyTwoQubitGate(7, 9, cnotGate);
    qsim.applyTwoQubitGate(3, 5, cnotGate);
    qsim.applyTwoQubitGate(1, 3, cnotGate);
    qsim.applySingleQubitGate(5, phaseGate);
    qsim.applyTwoQubitGate(5, 7, cnotGate);
  }

  EXPECT_NEAR(qsim.getProbability({1}, {false}), qsim.getExpectationValue({1}, {up}), eps);
  EXPECT_NEAR(qsim.getProbability({3}, {false}), qsim.getExpectationValue({3}, {up}), eps);
  EXPECT_NEAR(qsim.getProbability({5}, {false}), qsim.getExpectationValue({5}, {up}), eps);
  EXPECT_NEAR(qsim.getProbability({7}, {false}), qsim.getExpectationValue({7}, {up}), eps);
  EXPECT_NEAR(qsim.getProbability({9}, {false}), qsim.getExpectationValue({9}, {up}), eps);

  EXPECT_NEAR(qsim.getProbability({1}, {true}), qsim.getExpectationValue({1}, {down}), eps);
  EXPECT_NEAR(qsim.getProbability({3}, {true}), qsim.getExpectationValue({3}, {down}), eps);
  EXPECT_NEAR(qsim.getProbability({5}, {true}), qsim.getExpectationValue({5}, {down}), eps);
  EXPECT_NEAR(qsim.getProbability({7}, {true}), qsim.getExpectationValue({7}, {down}), eps);
  EXPECT_NEAR(qsim.getProbability({9}, {true}), qsim.getExpectationValue({9}, {down}), eps);

  EXPECT_NEAR(2*qsim.getProbability({1}, {false}), qsim.getExpectationValue({1}, {upDoubled}), eps);
}


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman
    Copyright (c) 2001-2010 Hartmut Kaiser

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A Calculator example demonstrating generation of AST which gets dumped into
//  a human readable format afterwards.
//
//  [ JDG April 28, 2008 ]
//  [ HK April 28, 2008 ]
//
///////////////////////////////////////////////////////////////////////////////

#if !defined(SPIRIT_EXAMPLE_CALC2_AST_APR_30_2008_1011AM)
#define SPIRIT_EXAMPLE_CALC2_AST_APR_30_2008_1011AM

#include <boost/variant.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/spirit/include/karma_domain.hpp>
#include <boost/spirit/include/support_attributes_fwd.hpp>

///////////////////////////////////////////////////////////////////////////////
//  Our AST
///////////////////////////////////////////////////////////////////////////////
struct binary_op;
struct unary_op;
struct nil {};

struct expression_ast
{
    typedef
        boost::variant<
            nil // can't happen!
          , int
          , boost::recursive_wrapper<binary_op>
          , boost::recursive_wrapper<unary_op>
        >
    type;

    // expose variant types
    typedef type::types types;

    // expose variant functionality
    int which() const { return expr.which(); }

    // constructors
    expression_ast()
      : expr(nil()) {}

    expression_ast(unary_op const& expr)
      : expr(expr) {}

    expression_ast(binary_op const& expr)
      : expr(expr) {}

    expression_ast(unsigned int expr)
      : expr(expr) {}

    expression_ast(type const& expr)
      : expr(expr) {}

    expression_ast& operator+=(expression_ast const& rhs);
    expression_ast& operator-=(expression_ast const& rhs);
    expression_ast& operator*=(expression_ast const& rhs);
    expression_ast& operator/=(expression_ast const& rhs);

    type expr;
};

// expose variant functionality
namespace boost
{
    // this function has to live in namespace boost for ADL to correctly find it
    template <typename T>
    inline T get(expression_ast const& expr)
    {
        return boost::get<T>(expr.expr);
    }

    // the specialization below tells Spirit to handle expression_ast as if it
    // where a 'real' variant
    namespace spirit { namespace traits
    {
        // the specialization below tells Spirit to handle expression_ast as
        // if it where a 'real' variant (if used with Spirit.Karma)
        template <>
        struct not_is_variant<expression_ast, karma::domain>
          : mpl::false_ {};

        // the specialization of variant_which allows to generically extract
        // the current type stored in the given variant like type
        template <>
        struct variant_which<expression_ast>
        {
            static int call(expression_ast const& v)
            {
                return v.which();
            }
        };
    }}
}

enum byte_code
{
    op_neg = 1,     //  negate the top stack entry
    op_pos,         //  essentially a no-op (unary plus)
    op_add,         //  add top two stack entries
    op_sub,         //  subtract top two stack entries
    op_mul,         //  multiply top two stack entries
    op_div,         //  divide top two stack entries
    op_int,         //  push constant integer into the stack
};

///////////////////////////////////////////////////////////////////////////////
struct binary_op
{
    binary_op() {}

    binary_op(
        int op
      , expression_ast const& left
      , expression_ast const& right)
      : op(op), left(left), right(right) {}

    int op;
    expression_ast left;
    expression_ast right;
};

struct unary_op
{
    unary_op(
        int op
      , expression_ast const& right)
    : op(op), right(right) {}

    int op;
    expression_ast right;
};

inline expression_ast& expression_ast::operator+=(expression_ast const& rhs)
{
    expr = binary_op(op_add, expr, rhs);
    return *this;
}

inline expression_ast& expression_ast::operator-=(expression_ast const& rhs)
{
    expr = binary_op(op_sub, expr, rhs);
    return *this;
}

inline expression_ast& expression_ast::operator*=(expression_ast const& rhs)
{
    expr = binary_op(op_mul, expr, rhs);
    return *this;
}

inline expression_ast& expression_ast::operator/=(expression_ast const& rhs)
{
    expr = binary_op(op_div, expr, rhs);
    return *this;
}

// We should be using expression_ast::operator-. There's a bug
// in phoenix type deduction mechanism that prevents us from
// doing so. Phoenix will be switching to BOOST_TYPEOF. In the
// meantime, we will use a phoenix::function below:
template <char Op>
struct unary_expr
{
    template <typename T>
    struct result { typedef T type; };

    expression_ast operator()(expression_ast const& expr) const
    {
        return unary_op(Op, expr);
    }
};

boost::phoenix::function<unary_expr<op_pos> > pos;
boost::phoenix::function<unary_expr<op_neg> > neg;

#endif


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
using namespace std;

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;

#include "../../../core/format_helper.h"
#include "../../base/stereo_sgm.h"

void runDenseStereo(const vector<string>& imagelist, const string& source_folder, const string& output_folder)
{
  StereoSGBM sgbm;

  // http://www.cvlibs.net/datasets/kitti/eval_stereo_flow_detail.php?benchmark=stereo&error=3&eval=all&result=3ae300a3a3b3ed3e48a63ecb665dffcc127cf8ab
  //sgbm.preFilterCap = 63;
  //sgbm.SADWindowSize = 3;       // 3, 5
  //int cn = 1;                   // image channel size
  ////sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
  //sgbm.P1 = 4*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;       // 50
  //sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;      // 800
  //sgbm.minDisparity = 0;
  //sgbm.numberOfDisparities = 128;     // 128, 256
  //sgbm.uniquenessRatio = 10;          // 10, 0
  //sgbm.speckleWindowSize = 100;       // 100
  //sgbm.speckleRange = 32;             // 32
  //sgbm.disp12MaxDiff = 1;
  //sgbm.fullDP = 1;

  // worse setting
  // http://www.cvlibs.net/datasets/kitti/eval_stereo_flow_detail.php?benchmark=stereo&error=3&eval=all&result=0d8c42f3fb90ff87bf89c1f8f7977cf309a0f49d
  //sgbm.preFilterCap = 15;
  //sgbm.SADWindowSize = 5;
  //sgbm.P1 = 50;
  //sgbm.P2 = 800;
  //sgbm.minDisparity = 0;
  //sgbm.numberOfDisparities = 256;
  //sgbm.uniquenessRatio = 0;
  //sgbm.speckleWindowSize = 100;
  //sgbm.speckleRange = 32;
  //sgbm.disp12MaxDiff = 1;
  //sgbm.fullDP = 1;

  // hand tuned
  sgbm.preFilterCap = 40;
  sgbm.SADWindowSize = 3;
  //sgbm.P1 = 40;
  sgbm.P1 = 40; // 10 good for 01 without histeq
  //sgbm.P2 = 2500; //1800;
  sgbm.P2 = 2500;
  sgbm.minDisparity = 0;
  sgbm.numberOfDisparities = 128; // or 256
  sgbm.uniquenessRatio = 0;
  sgbm.speckleWindowSize = 200;
  sgbm.speckleRange = 32;
  sgbm.disp12MaxDiff = 1;
  sgbm.fullDP = 1;

  vector<int> compression_params;
  compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
  compression_params.push_back(9);

  Mat img_left, img_right;
  Mat img_disp, img_disp_subpixel, mat_disp;
  for(size_t i = 0; i < imagelist.size(); i+=2) {
    cout << source_folder + imagelist[i] << "\n";
    img_left = imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
    img_right = imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);
    //cv::imshow("img_left", img_left);
    //cv::waitKey(0);

    double sigma = 0.7;
    cv::GaussianBlur(img_left, img_left, cv::Size(3,3), sigma);
    cv::GaussianBlur(img_right, img_right, cv::Size(3,3), sigma);

    recon::StereoSGMParams sgm_params;
    //sgm_params.disp_range = 128;
    //sgm_params.window_sz = 1;
    sgm_params.window_sz = 5;
    //sgm_params.penalty1 = 15;
    //sgm_params.penalty1 = 3;         // Daimler - 10
    //sgm_params.penalty2 = 100;
    sgm_params.penalty2 = 60;        // Daimler - 50
    // Axel tractor
    sgm_params.disp_range = 270;
    sgm_params.penalty1 = 7;

    recon::StereoSGM sgm(sgm_params);
    sgm.compute(img_left, img_right, mat_disp);
    mat_disp.convertTo(img_disp, CV_8U);

    //sgbm(img_left, img_right, img_disp);
    //img_disp.convertTo(img_disp_subpixel, CV_8U, 1.0/16.0);
    //img_disp.convertTo(mat_disp, CV_32F, 1.0/16.0);
    //img_disp.convertTo(img_disp, CV_8U, (255.0/sgbm.numberOfDisparities) * (1.0/16.0));

    string img_fname = output_folder + "img/disp_" + imagelist[i].substr(9, imagelist[i].size()-13) + ".png";
    string mat_fname = output_folder + "mat/disp_" + imagelist[i].substr(9, imagelist[i].size()-13) + ".yml";
    cout << "Saving: " << img_fname << "\n" << mat_fname << "\n----------------------\n";
    cv::equalizeHist(img_disp, img_disp);
    cv::imwrite(img_fname, img_disp, compression_params);
    cv::FileStorage mat_file(mat_fname, cv::FileStorage::WRITE);
    mat_file << "disparity_matrix" << mat_disp;

    //cout << img_disp << "\n\n";
    //imshow("disparity", img_disp);
    //waitKey(0);
  }
}

int main(int argc, char** argv)
{
  string config_file;
  string imagelistfn;
  string source_folder;
  string output_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    po::options_description config("Config file options");
    config.add_options()
      ("source_folder,s", po::value<string>(&source_folder), "folder with source")
      ("disp_folder,d", po::value<string>(&output_folder), "folder for output")
      ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
      ;

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).allow_unregistered().run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      cout << generic;
      cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      cout << "can not open config file: " << config_file << "\n";
      cout << generic;
      cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options, true), vm);
      notify(vm);
    }
    cout << "Configuring done, using:" << endl;

    if(vm.count("source_folder")) {
      cout << "Source folder: ";
      cout << source_folder << endl;
    }
    if(vm.count("disp_folder")) {
      cout << "Disparity folder: ";
      cout << output_folder << endl;
    }
    if(vm.count("imglist")) {
      cout << "Image list file: ";
      cout << imagelistfn << endl;
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return 1;
  }

  if(imagelistfn == "")
  {
    cout << "error: no xml image list given." << endl;
    return -1;
  }
  if(output_folder == "" || source_folder == "")
  {
    cout << "error: no output or source folder given." << endl;
    return -1;
  }

  vector<string> imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }

  runDenseStereo(imagelist, source_folder, output_folder);

  return 0;
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval_set_mixed unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/icl/interval_set.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

#include "../test_interval_set_mixed.hpp"

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_ctor_4_ordered_types)
{            interval_set_mixed_ctor_4_ordered_types<int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_equal_4_ordered_types)
{            interval_set_mixed_equal_4_ordered_types<std::string>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_assign_4_ordered_types)
{            interval_set_mixed_assign_4_ordered_types<float>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_ctor_4_bicremental_types)
{            interval_set_mixed_ctor_4_bicremental_types<bicremental_type_1>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_assign_4_bicremental_types)
{            interval_set_mixed_assign_4_bicremental_types<bicremental_type_2>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_equal_4_bicremental_types)
{            interval_set_mixed_equal_4_bicremental_types<bicremental_type_3>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_contains_4_bicremental_types)
{            interval_set_mixed_contains_4_bicremental_types<bicremental_type_4>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_add_4_bicremental_types)
{            interval_set_mixed_add_4_bicremental_types<bicremental_type_5>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_subtract_4_bicremental_types)
{            interval_set_mixed_subtract_4_bicremental_types<bicremental_type_6>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_erase_4_bicremental_types)
{            interval_set_mixed_erase_4_bicremental_types<bicremental_type_7>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_basic_intersect_4_bicremental_types)
{            interval_set_mixed_basic_intersect_4_bicremental_types<bicremental_type_8>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_disjoint_4_bicremental_types)
{            interval_set_mixed_disjoint_4_bicremental_types<bicremental_type_1>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_infix_plus_overload_4_bicremental_types)
{            interval_set_mixed_infix_plus_overload_4_bicremental_types<bicremental_type_2>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_infix_pipe_overload_4_bicremental_types)
{            interval_set_mixed_infix_pipe_overload_4_bicremental_types<bicremental_type_3>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_infix_minus_overload_4_bicremental_types)
{            interval_set_mixed_infix_minus_overload_4_bicremental_types<bicremental_type_4>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_infix_et_overload_4_bicremental_types)
{            interval_set_mixed_infix_et_overload_4_bicremental_types<bicremental_type_5>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_set_mixed_infix_caret_overload_4_bicremental_types)
{            interval_set_mixed_infix_caret_overload_4_bicremental_types<bicremental_type_6>(); }


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

  Copyright 2015 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/constant/nine.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check nine behavior for all types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
              (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::nine;
  using boost::simd::Nine;

  STF_TYPE_IS(decltype(Nine<T>()), T);
  STF_EQUAL(Nine<T>(), T(9));
  STF_EQUAL(nine( as(T{}) ),T(9));
}