#include <Functions/FunctionFactory.h>
#include <Functions/geometryConverters.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <base/logger_useful.h>

#include <Columns/ColumnArray.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnConst.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypeCustomGeo.h>

#include <memory>
#include <utility>

namespace DB
{

namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
}

template <typename Point>
class FunctionPolygonsSymDifference : public IFunction
{
public:
    static const char * name;

    explicit FunctionPolygonsSymDifference() = default;

    static FunctionPtr create(ContextPtr)
    {
        return std::make_shared<FunctionPolygonsSymDifference>();
    }

    String getName() const override
    {
        return name;
    }

    bool isVariadic() const override
    {
        return false;
    }

    size_t getNumberOfArguments() const override
    {
        return 2;
    }

    DataTypePtr getReturnTypeImpl(const DataTypes &) const override
    {
        return DataTypeFactory::instance().get("MultiPolygon");
    }

    bool isSuitableForShortCircuitArgumentsExecution(const DataTypesWithConstInfo & /*arguments*/) const override { return true; }

    ColumnPtr executeImpl(const ColumnsWithTypeAndName & arguments, const DataTypePtr & /*result_type*/, size_t input_rows_count) const override
    {
        MultiPolygonSerializer<Point> serializer;

        callOnTwoGeometryDataTypes<Point>(arguments[0].type, arguments[1].type, [&](const auto & left_type, const auto & right_type)
        {
            using LeftConverterType = std::decay_t<decltype(left_type)>;
            using RightConverterType = std::decay_t<decltype(right_type)>;

            using LeftConverter = typename LeftConverterType::Type;
            using RightConverter = typename RightConverterType::Type;

            if constexpr (std::is_same_v<ColumnToPointsConverter<Point>, LeftConverter> || std::is_same_v<ColumnToPointsConverter<Point>, RightConverter>)
                throw Exception(fmt::format("Any argument of function {} must not be Point", getName()), ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);
            else
            {
                auto first = LeftConverter::convert(arguments[0].column->convertToFullColumnIfConst());
                auto second = RightConverter::convert(arguments[1].column->convertToFullColumnIfConst());

                /// NOLINTNEXTLINE(clang-analyzer-core.uninitialized.Assign)
                for (size_t i = 0; i < input_rows_count; ++i)
                {
                    boost::geometry::correct(first[i]);
                    boost::geometry::correct(second[i]);

                    MultiPolygon<Point> sym_difference{};
                    boost::geometry::sym_difference(first[i], second[i], sym_difference);

                    serializer.add(sym_difference);
                }
            }
        });

        return serializer.finalize();
    }

    bool useDefaultImplementationForConstants() const override
    {
        return true;
    }
};

template <>
const char * FunctionPolygonsSymDifference<CartesianPoint>::name = "polygonsSymDifferenceCartesian";

template <>
const char * FunctionPolygonsSymDifference<SphericalPoint>::name = "polygonsSymDifferenceSpherical";

void registerFunctionPolygonsSymDifference(FunctionFactory & factory)
{
    factory.registerFunction<FunctionPolygonsSymDifference<CartesianPoint>>();
    factory.registerFunction<FunctionPolygonsSymDifference<SphericalPoint>>();
}

}


/* A simple server that generates keys for the different users */
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <openssl/bn.h>

#include <boost/algorithm/string.hpp>

#include "../lib/utils.h"
#include "../lib/kms.h"

int main(void)
{
    int key_size = SECURITY_KEYSIZE;
    int string_size = (key_size / 4 ) + 2;
    int sockfd, newsockfd, portno;
    socklen_t clilen;
    char buffer[string_size];
    struct sockaddr_in serv_addr, cli_addr;
    int n;

    KMS * kms = new KMS();
    kms->InitKMS(key_size);
    // We add a user by default
    kms->addUser();

    std::pair<BIGNUM *, BIGNUM *> keys = kms->keyGenKMS();

    sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd < 0)
        error("ERROR opening socket");

    bzero((char *) &serv_addr, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
    serv_addr.sin_port = htons(PORT_NUMBER);

    if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
          error("ERROR on binding");

    char * aux_component;
    while(1)
    {
        listen(sockfd, 5);
        clilen = sizeof(cli_addr);
        newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
        if (newsockfd < 0)
            error("ERROR on accept");
        bzero(buffer,(SECURITY_KEYSIZE/4)+1);
        n = read(newsockfd,buffer,(SECURITY_KEYSIZE/4)+1);
        if (n < 0) error("ERROR reading from socket");

        std::string * msg = new std::string(buffer);
        if (msg->compare("primeQ") == 0)
        {
            aux_component = BN_bn2hex(kms->getPrimeQ());
            //printf("Here is the primeQ: %s\n",aux_component);
            n = write(newsockfd,aux_component, string_size+1);
            if (n < 0) error("ERROR writing to socket");
        }
		else if(msg->compare("addUser") == 0)
        {
			unsigned int new_id = kms->addUser();

            const char * aux_c = std::to_string(new_id).c_str();
			aux_component = (char *) malloc(sizeof(char) * (strlen(aux_c) + 1));
			aux_component = strcpy(aux_component, aux_c);
            //printf("New User Id: %s\n",aux_component);
            n = write(newsockfd,aux_component,string_size+1);
            if (n < 0) error("ERROR writing to socket");
        }
        /*else if(msg->compare("masterK") == 0)
        {
            aux_component = BN_bn2hex(keys.first);
            printf("Here is the MasterK: %s\n",aux_component);
            n = write(newsockfd,aux_component,string_size+1);
            if (n < 0) error("ERROR writing to socket");
        }*/
        else if(msg->compare(0, 5, "userK") == 0)
        {
            /*aux_component = BN_bn2hex(keys.first);
            printf("Here is the UserK: %s\n",aux_component);
            n = write(newsockfd,aux_component,string_size+1);
            if (n < 0) error("ERROR writing to socket");*/
            std::vector<std::string> parts2;
			boost::split(parts2, *msg, boost::is_any_of(":"));
			if (parts2.size() == 2)
			{
				aux_component = kms->getServerKey(std::stoul (parts2[1],nullptr,0));
				//printf("Here is the UserK: %s\n",aux_component);
				n = write(newsockfd,aux_component, string_size+1);
				if (n < 0) error("ERROR writing to socket");
			}
            else
                error("The User Key is not being requested correctly");
        }
        else if(msg->compare(0, 7, "serverK") == 0)
        {
			std::vector<std::string> parts;
			boost::split(parts, *msg, boost::is_any_of(":"));
			if (parts.size() == 2)
			{
				aux_component = kms->getServerKey(std::stoul (parts[1],nullptr,0));
				//printf("Here is the ServerK: %s\n",aux_component);
				n = write(newsockfd,aux_component, string_size+1);
				if (n < 0) error("ERROR writing to socket");
			}
            else
                error("The server Key is not being requested correctly");
        }
        else // Check if the message begins with S or U
        {
            n = write(newsockfd,"puto", string_size);
            if (n < 0) error("ERROR writing to socket");
        }
		close(newsockfd);
        delete msg;
    }
    close(sockfd);
    return 0;
}


#define BOOST_TEST_MODULE "test_omp_3spn2_base_stacking_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/omp/System.hpp>
#include <mjolnir/omp/RandomNumberGenerator.hpp>
#include <mjolnir/omp/ThreeSPN2BaseStackingInteraction.hpp>
#include <mjolnir/omp/ThreeSPN2BaseStackingInteraction.hpp>
#include <mjolnir/input/read_units.hpp>

BOOST_AUTO_TEST_CASE(OpenMP_ThreeSPN2BaseStackingInteraction)
{
    namespace test = mjolnir::test;

    mjolnir::LoggerManager::set_default_logger(
            "test_omp_3spn2_base_stacking_interaction.log");

    using seq_traits_type = mjolnir::SimulatorTraits      <double, mjolnir::UnlimitedBoundary>;
    using omp_traits_type = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type       = typename omp_traits_type::real_type;
    using coordinate_type = typename omp_traits_type::coordinate_type;
    using boundary_type   = typename omp_traits_type::boundary_type;

    using seq_system_type = mjolnir::System<seq_traits_type>;
    using omp_system_type = mjolnir::System<omp_traits_type>;

    using potential_type  = mjolnir::ThreeSPN2BaseStackingPotential<real_type>;
    using base_stack_kind = typename potential_type::base_stack_kind;

    using omp_interaction_type = mjolnir::ThreeSPN2BaseStackingInteraction<omp_traits_type>;
    using seq_interaction_type = mjolnir::ThreeSPN2BaseStackingInteraction<seq_traits_type>;

    using ParameterSet = mjolnir::ThreeSPN2BaseStackingPotentialParameter<real_type>;
    {
        using unit_type = mjolnir::unit::constants<real_type>;
        using phys_type = mjolnir::physics::constants<real_type>;
        const std::string energy = "kcal/mol";
        const std::string length = "angstrom";
        phys_type::set_kB(phys_type::kB() * (unit_type::J_to_cal() / 1000.0) *
                          unit_type::avogadro_constant());
        phys_type::set_eps0(phys_type::eps0() * (1000.0 / unit_type::J_to_cal()) /
                            unit_type::avogadro_constant());
        phys_type::set_energy_unit(energy);

        phys_type::set_eps0(phys_type::eps0() / unit_type::m_to_angstrom());

        phys_type::set_m_to_length(unit_type::m_to_angstrom());
        phys_type::set_length_to_m(unit_type::angstrom_to_m());

        phys_type::set_L_to_volume(1e-3 * std::pow(unit_type::m_to_angstrom(), 3));
        phys_type::set_volume_to_L(1e+3 * std::pow(unit_type::angstrom_to_m(), 3));

        phys_type::set_length_unit(length);
    }

    //        SBi
    //     Si --> Bi
    //    /     `-^
    //   Pj theta | rij
    //    \       |
    //     Sj --- Bj
    //
    //  rij:
    //  1. (r < r0)
    //  2. (r0 < r)
    //  theta:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta

    std::mt19937 rng(123456789);

    const auto bs_kind = base_stack_kind::AA;

    potential_type   potential(ParameterSet{});
    omp_interaction_type omp_interaction("none",
            std::vector<std::pair<std::array<std::size_t, 3>, base_stack_kind>>{
                { {{0, 1, 2}}, bs_kind }
            }, potential_type(potential), {});
    seq_interaction_type seq_interaction("none",
            std::vector<std::pair<std::array<std::size_t, 3>, base_stack_kind>>{
                { {{0, 1, 2}}, bs_kind }
            }, potential_type(potential), {});

    omp_system_type omp_sys(3, boundary_type{});
    seq_system_type seq_sys(3, boundary_type{});

    test::clear_everything(omp_sys);
    test::clear_everything(seq_sys);

    omp_sys.name(0)  = "Si";
    omp_sys.name(1)  = "Bi";
    omp_sys.name(2)  = "Bj";
    omp_sys.group(0) = "DNA";
    omp_sys.group(1) = "DNA";
    omp_sys.group(2) = "DNA";

    potential.initialize(omp_sys);

    omp_interaction.initialize(omp_sys);
    seq_interaction.initialize(seq_sys);

    const auto theta0    = potential.theta_0(bs_kind);
    const auto pi_over_K = potential.pi_over_K_BS();
    const auto theta0_1  = theta0 + 0.2 * pi_over_K; //         dtheta < pi/2K
    const auto theta0_2  = theta0 + 0.7 * pi_over_K; // pi/2K < dtheta < pi/K
    const auto theta0_3  = theta0 + 1.2 * pi_over_K; // pi/K  < dtheta
    const auto r0_1      = potential.r0(bs_kind) - 0.2;
    const auto r0_2      = potential.r0(bs_kind) + 0.5;

    for(const auto r : {r0_1, r0_2})
    {
    for(const auto theta : {theta0_1, theta0_2, theta0_3})
    {
        BOOST_TEST_MESSAGE("======================================");
        BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta);

    for(std::size_t i=0; i<100; ++i)
    {
        // generate particle configuration in the following way
        //    y
        // Bj ^
        //  \ | theta0
        // r0\|-.
        // ---o-----o--> x
        //  Bi      Si
        //

        omp_sys.position(0) = coordinate_type(4.0, 0.0, 0.0); // Si
        omp_sys.position(1) = coordinate_type(0.0, 0.0, 0.0); // Bi
        omp_sys.position(2) = coordinate_type(r * std::cos(theta), r * std::sin(theta), 0.0); // Bj

        test::apply_random_rotation(omp_sys, rng);
        test::apply_random_perturbation(omp_sys, rng, 0.01);
        test::clear_force(omp_sys);
        test::clear_force(seq_sys);

        seq_sys.position(0) = omp_sys.position(0);
        seq_sys.position(1) = omp_sys.position(1);
        seq_sys.position(2) = omp_sys.position(2);

        constexpr real_type tol = 1e-4;

        test::check_force_consistency              (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_energy_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_virial_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_energy_virial_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_energy_consistency             (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
    } // perturbation
    } // theta
    } // r
}


#define BOOST_TEST_MAIN TestTopK
#include <boost/test/unit_test.hpp>
#include <stdexcept>
#include <iostream>
#include <sam/TestProducers.hpp>
#include <sam/TopK.hpp>
#include <sam/Filter.hpp>
#include <sam/Expression.hpp>
#include <sam/VastNetflow.hpp>

using namespace sam;

struct F
{
  std::shared_ptr<FeatureMap> featureMap;
  string identifier = "topk";
  int N = 10000;
  int b = 1000;
  int k = 3;

  F()
  {
    featureMap = std::make_shared<FeatureMap>();
  }
};

BOOST_FIXTURE_TEST_CASE( test_topk_no_key, F )
{
  size_t queueLength = 1000;
  size_t numPopular = 2;
  size_t numExamples = 100000;
  double probabilityPop = 0.5;

  PopularSites producer(queueLength, numExamples, numPopular,
                        probabilityPop);

  auto topk = std::make_shared<TopK<VastNetflow, DestIp>>
    (N, b, k, 0, featureMap, identifier);

  producer.registerConsumer(topk);

  producer.run();

  std::shared_ptr<Feature const> feature = featureMap->at("", identifier);

  auto func0 = [](Feature const * feature)->double {
    auto topKFeature = static_cast<TopKFeature const *>(feature);
    return topKFeature->getFrequencies()[0];
  };
  auto func1 = [](Feature const * feature)->double {
    auto topKFeature = static_cast<TopKFeature const *>(feature);
    return topKFeature->getFrequencies()[1];
  };

  double frequency0 = feature->evaluate<double>(func0);
  double frequency1 = feature->evaluate<double>(func0);
  BOOST_CHECK_CLOSE(frequency0, 0.25, 5);
  BOOST_CHECK_CLOSE(frequency1, 0.25, 5);
}

BOOST_FIXTURE_TEST_CASE( test_topk_server, F )
{
  int queueLength = 1000;
  int numExamples = 100000;
  int numServers = 2;
  int numNonservers = 2;

  // The TopKProducer creates a situation where there are numServers servers
  // and numNonservers non-servers.  A server is defined as > 90% of traffic
  // to the top two ports.
  TopKProducer producer(queueLength, numExamples, numServers, numNonservers);

  // Creating the topk computation and registering it as a consumer
  // of the data source: producer.
  auto topk = std::make_shared<TopK<VastNetflow, DestPort, DestIp>>
    (N, b, k, 0, featureMap, identifier);
  producer.registerConsumer(topk);

  // TODO: The below section creating the filter isn't actually tested.
  // Add more tests to see that the filter works.

  /////// Creating the filter expression /////////////////////////
  // We create an infix expression to filter for servers, i.e.
  //  Servers = FILTER VertsByDest BY top2.value(0) + top2.value(1) < 0.9;
  // The infix is the part after the BY.

  // First function token
  auto function1 = [](Feature const * feature)->double {
    auto topKFeature = static_cast<TopKFeature const *>(feature);
    return topKFeature->getFrequencies()[0];
  };
  auto funcToken1 = std::make_shared<FuncToken<VastNetflow>>(featureMap, function1,
                                                         identifier);

  // Addition token
  auto addOper = std::make_shared<AddOperator<VastNetflow>>(featureMap);

  // Second function token
  auto function2 = [](Feature const * feature)->double {
    auto topKFeature = static_cast<TopKFeature const *>(feature);
    return topKFeature->getFrequencies()[1];
  };
  auto funcToken2 = std::make_shared<FuncToken<VastNetflow>>(featureMap, function2,
                                                         identifier);

  // Lessthan token
  auto lessThanToken = std::make_shared<LessThanOperator<VastNetflow>>(featureMap);

  // Number token
  auto numberToken = std::make_shared<NumberToken<VastNetflow>>(featureMap, 0.9);

  std::list<std::shared_ptr<ExpressionToken<VastNetflow>>> infixList;
  infixList.push_back(funcToken1);
  infixList.push_back(addOper);
  infixList.push_back(funcToken2);
  infixList.push_back(lessThanToken);
  infixList.push_back(numberToken);

  auto filterExpression = std::make_shared<Expression<VastNetflow>>(infixList);
  auto filter = std::make_shared<Filter<VastNetflow, DestIp>>(
    filterExpression, 0, featureMap, "servers", queueLength);

  producer.registerConsumer(filter);

  producer.run();
  for (std::string ip : producer.getServerIps()) {
    std::shared_ptr<Feature const> feature = featureMap->at(ip, identifier);
    int index1 = 0;
    auto function1 = [&index1](Feature const * feature)->double {
      auto topKFeature = static_cast<TopKFeature const *>(feature);
      return topKFeature->getFrequencies()[index1];
    };

    // TopKProducer sends data in a uniform random way to two ports to the
    // server ips, so the top two values should both be about 0.5 frequency.
    double value = feature->evaluate<double>(function1);
    BOOST_CHECK_CLOSE(value, 0.5, 0.01);

    index1 = 1;
    value = feature->evaluate<double>(function1);
    BOOST_CHECK_CLOSE(value, 0.5, 0.01);
  }
  for (std::string ip : producer.getNonserverIps()) {
    std::shared_ptr<Feature const> feature = featureMap->at(ip, identifier);
    std::vector<double> parameters;
    parameters.push_back(0);

    int index1 = 0;
    auto function1 = [&index1](Feature const * feature)->double {
      auto topKFeature = static_cast<TopKFeature const *>(feature);
      return topKFeature->getFrequencies()[index1];
    };

    // TopKProducer sends data in a uniform random way to three ports
    // to the non-server ips, so the top three values should all be about 0.33
    double value = feature->evaluate<double>(function1);
    BOOST_CHECK_CLOSE(value, 0.333333, 0.01);

    index1 = 1;
    value = feature->evaluate<double>(function1);
    BOOST_CHECK_CLOSE(value, 0.333333, 0.01);

    index1 = 2;
    value = feature->evaluate<double>(function1);
    BOOST_CHECK_CLOSE(value, 0.333333, 0.01);
  }
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <string>
#include <utility>
#include <Eigen/Core>
#include <limits>
#include <cassert>
#include "metro/regression/LogLikelihood.hpp"
#include "metro/ModifiedCholesky.hpp"
#include "metro/ModifiedNewtonRaphson.hpp"
#include "metro/fit_model.hpp"

#define USE_MODIFIED_NR 1
#define DEBUG 1

namespace metro {
	Stepper::~Stepper() {}

	std::pair< bool, int > fit_model(
		metro::SmoothFunction& ll,
		std::string const& model_name,
		Eigen::VectorXd const& starting_point,
		Stepper& stopping_condition,
		std::vector< std::string >* comments
	) {
		typedef metro::regression::Design::Matrix Matrix ;
		// Fit null model
		assert( starting_point.size() == ll.number_of_parameters() ) ;
		ll.evaluate_at( starting_point ) ;

		bool success = true ;
		int iterations = 0 ;
		// Compute negative definiteness condition number.
		Eigen::SelfAdjointEigenSolver< Matrix > eigenSolver( ll.get_value_of_second_derivative() ) ;
		if( eigenSolver.eigenvalues().maxCoeff() > 0 ) {
			success = false ;
			if( comments ) {
				comments->push_back( model_name + ":not-negative-definite" ) ;
			}
		} else {
#if 1
			Eigen::VectorXd parameters = metro::find_maximum_by_modified_newton_raphson_with_line_search(
				ll, starting_point, stopping_condition
			) ;
#else
			metro::Snptest25StoppingCondition< metro::regression::LogLikelihood > stopping_condition(
				ll,
				0.01,
				100,
				0
				//&get_ui_context().logger()
			) ;

			Eigen::ColPivHouseholderQR< Matrix > solver ;
			Eigen::VectorXd parameters = maximise_by_newton_raphson( ll, starting_point, stopping_condition, solver ) ;
#endif

			if( stopping_condition.diverged() ) {
				if( comments ) {
					comments->push_back( model_name + ":model_fit_error:failed_to_converge_to_mle" ) ;
				}
			}

			success = !stopping_condition.diverged() ;
			iterations = stopping_condition.number_of_iterations() ;
		}

		return std::make_pair( success, iterations ) ;
	}
}


/*===========================================================================*\
 *                                                                           *
 *                               CoMISo                                      *
 *      Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen      *
 *                           www.rwth-graphics.de                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 *  This file is part of CoMISo.                                             *
 *                                                                           *
 *  CoMISo is free software: you can redistribute it and/or modify           *
 *  it under the terms of the GNU General Public License as published by     *
 *  the Free Software Foundation, either version 3 of the License, or        *
 *  (at your option) any later version.                                      *
 *                                                                           *
 *  CoMISo is distributed in the hope that it will be useful,                *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the            *
 *  GNU General Public License for more details.                             *
 *                                                                           *
 *  You should have received a copy of the GNU General Public License        *
 *  along with CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
 *                                                                           *
\*===========================================================================*/

#include <CoMISo/Config/config.hh>
#include <vector>
#include <cstdlib>
#include <iostream>

//------------------------------------------------------------------------------------------------------
#if COMISO_SUITESPARSE_SPQR_AVAILABLE // additional spqr library required
//------------------------------------------------------------------------------------------------------

#include <CoMISo/Utils/StopWatch.hh>
#include <Eigen/Sparse>
#include <CoMISo/Solver/SparseQRSolver.hh>
#include <CoMISo/Solver/Eigen_Tools.hh>

//------------------------------------------------------------------------------------------------------

// Example main
int main(void)
{
  std::cout << "---------- 0) Using Sparse QR for solving underdetermined equations and computing Null spaces " << std::endl;

  typedef Eigen::SparseMatrix< double > SpMatrix;
  typedef Eigen::MatrixXd DenMatrix;
  typedef Eigen::Triplet< double > Triplet;

  using COMISO_GMM::operator<<;

  int dimr(4+1);
  int dimc(4+2);

  std::cout << "---------- 1) Creating matrix " << std::endl;
  std::vector< Triplet > triplets;
  for( int i = 0; i < dimc*dimr/2; ++i)
  {
    int x( rand()%(dimr-1));
    int y( rand()%dimc);
    double val( rand()%10);
    //std::cerr << " setting (" << x << ", " << y << ") to " << val << std::endl;
    triplets.push_back( Triplet( x, y, val));
  }
  SpMatrix A(dimr,dimc);
  A.setFromTriplets(triplets.begin(), triplets.end());

  std::cerr << DenMatrix(A) << std::endl;
  int m = dimr;
  int n = dimc;

  if( m < n )
  {
    std::swap( m,n);
    std::cerr << " ... m < n -> form transposed ..." << std::endl;
    A = SpMatrix(A.transpose());
    // test make also row -rank-deficinet
    A.middleCols(n-1,1) = A.middleCols(0,1);
    A.middleCols(0,1) = A.middleCols(n-2,1);
    std::cerr << DenMatrix(A) << std::endl;
  }

  std::cerr << " ... m = " << m << "; n = " << n << std::endl;
  std::cerr << std::endl;

  std::cout << "---------- 2) Sparse QR " << std::endl;
  COMISO::SparseQRSolver spqr;
  SpMatrix Q,R;
  std::vector< size_t > P;
  int rank = spqr.factorize_system_eigen( A, Q, R, P);
  int nullity(dimc-rank);
  // setup permutation matrix
  SpMatrix Pm( n, n);
  if( !P.empty())
  {
    for( size_t i = 0; i < P.size(); ++i)
    {
      Pm.coeffRef( (int)i, (int)P[i]) = 1;
    }
  }

  std::cout << "---------- 3) Result " << std::endl;
  std::cerr << " Q         " << std::endl << DenMatrix(Q) << std::endl;
  std::cerr << " R         " << std::endl << DenMatrix(R) << std::endl;
  std::cerr << " P         " << std::endl << P << std::endl;
  std::cerr << " P matrix  " << std::endl << DenMatrix(Pm) << std::endl;
  std::cerr << " Rank      " << rank << std::endl;
  std::cerr << " Nullity   " << nullity << std::endl;
  // extract nullspace
  SpMatrix NullSpace( Q.middleCols( std::max( 0, m-nullity), nullity));
  std::cerr << " Nullspace " << std::endl << DenMatrix(NullSpace) << std::endl;
  // non nullspace part of R
  //// assuming superflous column in R is the last (if A is also row deficient)
  //SpMatrix Rtmp(R.middleCols(0,std::min(n,n-(n-rank))).transpose());
  //SpMatrix R1( R.transpose().middleCols(0, m-nullity));
  SpMatrix Rtmp(R.transpose());
  SpMatrix R1t( Rtmp.middleCols(0,m-nullity));
  SpMatrix R1( R1t.transpose());
  std::cerr << " Non-Nullspace R " << std::endl << DenMatrix(R1) << std::endl;

  std::cout << "---------- 4) Verification " << std::endl;
  SpMatrix reconstructedA(Q*R*Pm.transpose());
  std::cerr << " Q orthogonal? \t " << ((fabs((Q.transpose()*Q).squaredNorm()-m) < 1e-8)?"yes":"no") << std::endl;
  std::cerr << " A = QR?       \t " << (((reconstructedA-A).squaredNorm() < 1e-8)? "yes":"no") << std::endl;

  std::cerr << std::endl << std::endl;
  std::cout << "---------- 5) Solving Ax=b (with x without nullspace component)" << std::endl;
  // NOTE: A was transposed above to be m>n
  SpMatrix b(n,1);
  SpMatrix x(m,1);
  for( int i = 0; i < n; ++i)
    b.coeffRef(i,0) = rand()%10;
  std::cerr << " ... System Ax = b .. \n";
  std::cerr << " A " << std::endl << DenMatrix(A.transpose()) << " x " << std::endl << DenMatrix(x) << " b " << std::endl << DenMatrix(b) << std::endl;

  std::cout << "---------- 5.1) test: solve using sparse QR solving .." << std::endl;
  SpMatrix At(A.transpose());
  spqr.solve_system_eigen( At, b, x);

  std::cerr << " ... solution x .. " << std::endl;
  std::cerr << DenMatrix(x) << std::endl;

  std::cerr << " ... test: is a solution ? " << (((A.transpose()*x-b).squaredNorm()<1e-8)?"yes":"no") << std::endl;
  std::cerr << " ... test: has nullspace component ? " << ((x.transpose()*NullSpace).squaredNorm()<1e-8?"yes":"no") << std::endl;
  std::cerr << " ... Nullspace projections : " << (x.transpose()*NullSpace) << std::endl;

  std::cout << "---------- 5.2) test: solve without nullspace .." << std::endl;
  SpMatrix Atnull(At);
  SpMatrix bnull(b);
  SpMatrix xnull(m,1);
  spqr.solve_system_eigen_min2norm( Atnull, bnull, xnull);
  std::cerr << " ... solution x .. " << std::endl;
  std::cerr << DenMatrix(xnull) << std::endl;

  std::cerr << " ... test: is a solution ? " << (((A.transpose()*xnull-bnull).squaredNorm()<1e-8)?"yes":"no") << std::endl;
  std::cerr << " ... test: has nullspace component ? " << ((xnull.transpose()*NullSpace).squaredNorm()<1e-8?"yes":"no") << std::endl;
  std::cerr << " ... Nullspace projections : " << (xnull.transpose()*NullSpace) << std::endl;

  return 0;
}

#else // COMISO_SUITESPARSE_SPQR_AVAILABLE

int main(void)
{
  std::cerr << " SUITESPARSE_SPQR not available, please re-configure!\n";
  return 0;
}

#endif  // COMISO_SUITESPARSE_SPQR_AVAILABLE


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the secant of the angle in radian: \f$1/\cos(x)\f$.

    @see cos, secd, secpi

    @par Header <boost/simd/function/sec.hpp>

    @par Example:

      @snippet sec.cpp sec

    @par Possible output:

      @snippet sec.txt sec

  **/
  IEEEValue sec(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sec.hpp>
#include <boost/simd/function/simd/sec.hpp>

#endif


#ifndef COMMONTOOLS_HH
#define COMMONTOOLS_HH

#include <boost/filesystem.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <sstream>
#include <fstream>
#include <cmath>

typedef boost::filesystem::path path_t;
typedef std::list<path_t> result_set_t;

int lowpass(double x, double &y, double i, double a);
int derivation( double x, double & dx, double i);

int dumpData(std::string fileName, std::vector< std::vector<double> >& data);

void getOptions(int argc,
                char *argv[],
                path_t &dataRootPath,
                path_t &outputFile);

class InfiniteImpendanceFilter
{
public :
  InfiniteImpendanceFilter()
  {
    // Create the window for the filter.
    m_SamplingPeriod = 0.005 ;
    double T=0.05; // Arbritrary from Kajita's San Matlab files.
    int n=0;
    double sum=0,tmp=0;

    assert(m_SamplingPeriod > 0);
    n = (int)floor(T/m_SamplingPeriod);
    m_filterWindow.resize(n+1);
    for(int i=0;i<n+1;i++)
      {
        tmp =sin((M_PI*i)/n);
        m_filterWindow[i]=tmp*tmp;
      }

    for(int i=0;i<n+1;i++)
      sum+= m_filterWindow[i];

    for(int i=0;i<n+1;i++)
      m_filterWindow[i]/= sum;
  }

  void filter(std::vector< std::vector<double> > & in,
              std::vector< std::vector<double> > & out)
  {
    for(unsigned j=0 ; j<in[0].size() ; ++j)
    {
      int n=0;
      double T=0.050; // Arbritraty fixed from Kajita's San matlab files.

      assert(out.size()==in.size()&&out[0].size()==in[0].size());

      // Creates window.
      assert(m_SamplingPeriod > 0);
      n = (int)floor(T/m_SamplingPeriod);

      // Filter vector.
      for(int i=0;i<n+1;i++)
      {
        out[i][j] = in[i][j];
      }

      for(unsigned int i=n+1;i<in.size();i++)
      {
        double ltmp = 0.0 ;
        for(unsigned int k=0;k<m_filterWindow.size();k++)
        {
          ltmp += m_filterWindow[k]*in[i-k][j];
        }
        out[i][j] = ltmp ;
      }
    }
  }
private :
  std::vector<double> m_filterWindow;
  double m_SamplingPeriod ;
};

#endif // COMMONTOOLS_HH


#ifndef STAN_MATH_MPI_VAR_ESTIMATOR_HPP
#define STAN_MATH_MPI_VAR_ESTIMATOR_HPP

#ifdef STAN_LANG_MPI

#include <stan/math/torsten/mpi/environment.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/mpi.hpp>
#include <algorithm>
#include <vector>

namespace stan {
namespace math {
  namespace mpi {

class mpi_var_estimator {
  using acc_param = boost::accumulators::accumulator_set<double,
                                                         boost::accumulators::stats<
                                                         boost::accumulators::tag::sum,
                                                         boost::accumulators::tag::mean,
                                                         boost::accumulators::tag::variance>>;
  using acc_count = boost::accumulators::accumulator_set<int,
                                                         boost::accumulators::features<boost::accumulators::tag::count>>;

 public:
  explicit mpi_var_estimator(size_t num_params)
    : acc_(num_params)
  {}

  void restart() {
    for (size_t i = 0; i < acc_.size(); ++i) {
      acc_[i] = {};
    }
    count_acc_ = {};
  }

  void restart(size_t num_params) {
    acc_.resize(num_params);
    restart();
  }

  inline void add_sample(const Eigen::VectorXd& q) {
    for (size_t i = 0; i < acc_.size(); ++i) {
      acc_[i](q(i));
    }
    count_acc_(0);
  }

  inline std::vector<int> num_samples(const Communicator& comm) {
    int n = boost::accumulators::count(count_acc_);
    int n_all;
    MPI_Allreduce(&n, &n_all, 1, MPI_INT, MPI_SUM, comm.comm());
    return {n, n_all};
  }

  inline void sample_mean(Eigen::VectorXd& var, int n_all, const Communicator& comm) {
    int num_params = acc_.size();
    Eigen::ArrayXd work(num_params);
    var.resize(num_params);
    for (int i = 0; i < num_params; ++i) {
      work(i) = boost::accumulators::sum(acc_[i]);
    }
    MPI_Allreduce(work.data(), var.data(), num_params, MPI_DOUBLE, MPI_SUM, comm.comm());
    var /= n_all;
  }

  inline void sample_mean(Eigen::VectorXd& var, const Communicator& comm) {
    int n_all = num_samples(comm)[1];
    sample_mean(var, n_all, comm);
  }

  inline int sample_variance(Eigen::VectorXd& var, const Communicator& comm) {
    std::vector<int> n_samples(num_samples(comm));
    int n = n_samples[0];
    int n_all = n_samples[1];
    int m = comm.size();
    int num_params = acc_.size();
    Eigen::VectorXd work(num_params);
    sample_mean(work, n_all, comm);
    for (int i = 0; i < num_params; ++i) {
      double d = boost::accumulators::mean(acc_[i]) - work(i);
      work(i) = n * boost::accumulators::variance(acc_[i]) + n * d * d;
    }
    var.resize(num_params);
    MPI_Allreduce(work.data(), var.data(), num_params, MPI_DOUBLE, MPI_SUM, comm.comm());
    var /= (n_all - 1.0);
    return n_all;
  }

  inline size_t num_params() {
    return acc_.size();
  }

 protected:
  std::vector<acc_param> acc_;
  acc_count count_acc_;
};

  }
}  // namespace math
}  // namespace stan

#endif

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/sqrtvalmax.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/sqr.hpp>

#ifdef USE_UBSAN
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_unsigned.hpp>
#endif

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

  BOOST_DISPATCH_OVERLOAD_IF ( sqr_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::multiplies(a0, a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(sqr_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::int_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &,  const A0& a0
                                   ) const BOOST_NOEXCEPT
    {
      // workaround for UBSan
#ifdef USE_UBSAN
      using utype = bs::as_unsigned_t<A0>;
      return if_else(is_greater(saturated_(abs)(a0), bs::Sqrtvalmax<A0>()),
                     bs::Valmax<A0>(), bitwise_cast<A0>(sqr(bitwise_cast<utype>(a0))));
#else
      return if_else(is_greater(saturated_(abs)(a0), bs::Sqrtvalmax<A0>()),
                     bs::Valmax<A0>(), sqr(a0));
#endif
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF(sqr_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::uint_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &,  const A0& a0
                                   ) const BOOST_NOEXCEPT
    {
      return if_else(is_greater(a0, bs::Sqrtvalmax<A0>()),
                     bs::Valmax<A0>(), sqr(a0));
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF(sqr_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &,  const A0& a0
                                   ) const BOOST_NOEXCEPT
    {
      return sqr(a0);
    }
  };

} } }

#endif


#include "libPSI/config.h"
#ifdef ENABLE_PRTY_PSI
#include "polyFFT2.h"
#include <cryptoTools/Common/Log.h>
#include <cryptoTools/Common/Timer.h>
#include <cryptoTools/Crypto/RandomOracle.h>
#include <vector>
#include <thread>
#include <NTL/BasicThreadPool.h>
#include <omp.h>
namespace osuCrypto
{
#define LEFT(X) (2*X+1)
#define RIGHT(X) (2*X+2)
#define PAPA(X) ((X-1)/2)

	void polyFFT2::init(ZZ &prime, u64 numThreads) {
		mPrime = prime;
		mNumThreads = numThreads;
	}

	void build_tree_1thread(ZZ_pX* tree, ZZ_p* points) {

	}

	void polyFFT2::build_tree(ZZ_pX* tree, ZZ_p* points) {

	}

}
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IDIVROUND_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IDIVROUND_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/idivround.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/simd/include/functions/simd/iround.hpp>
#include <boost/simd/include/functions/simd/iround.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return iround(tofloat(a0)/tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))((simd_<unsigned_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return rdivide(a0+a1/boost::simd::Two<A0>(), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<int16_<A0>,X>))((simd_<int16_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype,X>                 ivtype;
      ivtype a0l, a0h, a1l, a1h;
      split(a0, a0l, a0h );
      split(a1, a1l, a1h);
      return simd::bitwise_cast<A0>(group(idivround(a0l, a1l),
                               idivround(a0h, a1h)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<int8_<A0>,X>))((simd_<int8_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype, X>                 ivtype;
      ivtype a0l, a0h, a1l, a1h;
      split(a0, a0l, a0h);
      split(a1, a1l, a1h);
      return simd::bitwise_cast<A0>(group(idivround(a0l, a1l),
                               idivround(a0h, a1h) ));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::idivround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    { return iround(a0/a1); }
  };
} } }

#endif


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include <kugle_misc/VelocityLQR.h>

#include <math.h>
#include <stdlib.h>
#include <cmath>

#include <ros/ros.h>
#include <boost/thread/thread.hpp>
#include <kugle_misc/Quaternion.h>
#include <kugle_misc/FirstOrderLPF.h>

namespace kugle_misc {

VelocityLQR::VelocityLQR(double Rate) : rate_(Rate), _omega_x_ref_filt(1.0/Rate, params_.OmegaLPFtau), _omega_y_ref_filt(1.0/Rate, params_.OmegaLPFtau), gainMatrix(Eigen::Map<const Eigen::Matrix<double, 2, 10, Eigen::RowMajor>>(const_cast<double*>(gainMatrixCoefficients)))
{
    ros::NodeHandle nParam("~");

    // Publish to quaternion reference topic
    quaternionPub_ = nh_.advertise<geometry_msgs::Quaternion>("cmd_quaternion", 1000);

    // Subscribe to state estimate topic
	stateEstimateSub_ = nh_.subscribe("StateEstimate", 1000, &VelocityLQR::StateEstimateCallback, this);

    // Subscribe to velocity reference
    referenceSub_ = nh_.subscribe("cmd_vel", 1000, &VelocityLQR::VelocityReferenceCallback, this);

    // Subscribe to velocity reference
    referenceInertialSub_ = nh_.subscribe("cmd_vel_inertial", 1000, &VelocityLQR::VelocityReferenceInertialCallback, this);

    // Load parameters
    /*nParam.param("velocity_error_clamp", params_.VelocityErrorClamp, params_.VelocityErrorClamp);
    nParam.param("max_tilt", params_.MaxTilt, params_.MaxTilt);
    nParam.param("integral_gain", params_.IntegralGain, params_.IntegralGain);
    nParam.param("max_integral_correction", params_.MaxIntegralCorrection, params_.MaxIntegralCorrection);*/

    _omega_x_ref_filt.ChangeTimeconstant(params_.OmegaLPFtau);
    _omega_y_ref_filt.ChangeTimeconstant(params_.OmegaLPFtau);

    // Reset internal module states
    currentStateEstimate.q.w = 1;
    currentStateEstimate.q.x = 0;
    currentStateEstimate.q.y = 0;
    currentStateEstimate.q.z = 0;
    currentStateEstimate.dq.w = 0;
    currentStateEstimate.dq.x = 0;
    currentStateEstimate.dq.y = 0;
    currentStateEstimate.dq.z = 0;
    currentStateEstimate.position[0] = 0;
    currentStateEstimate.position[1] = 0;
    currentStateEstimate.velocity[0] = 0;
    currentStateEstimate.velocity[1] = 0;
    velocityRefGivenInHeadingFrame_ = true;
    desiredVelocity_.x = 0;
    desiredVelocity_.y = 0;
    desiredAngularYawVelocity_ = 0;
    lastReferenceUpdateTime_ = ros::Time::now();

    // Reset roll and pitch reference variables (used to integrate the computed angular velocity reference)
    roll_ref = 0;
    pitch_ref = 0;

    position_control_enabled = false;

    // Reset position reference used for station keeping
    position_reference[0] = 0;
    position_reference[1] = 0;

    // Reset velocity error integral
    velocity_error_integral[0] = 0;
    velocity_error_integral[1] = 0;

    VelocityIntegralAfterPowerup = true;
    VelocityIntegralInitializationTime = 5; // wait 5 seconds in the beginning for integrator to settle (and before allowing manual movement)

    shouldExit_ = false;
    thread_ = boost::thread(boost::bind(&VelocityLQR::Thread, this));
}

VelocityLQR::~VelocityLQR()
{
    shouldExit_ = true;
    thread_.join();
}

void VelocityLQR::Thread()
{
    ros::Rate loop_rate(rate_); // 10 hz loop rate
    double dt = 1.0 / rate_;

    geometry_msgs::Quaternion quaternionMsg;
    double desiredYaw = 0;
    double q[4];
	double dq[4];
    double xy[2];
    double dxy[2];
    double velocityRef[2];
    double q_ref[4];

    while (ros::ok() && !shouldExit_)
    {
        q[0] = currentStateEstimate.q.w;
        q[1] = currentStateEstimate.q.x;
        q[2] = currentStateEstimate.q.y;
        q[3] = currentStateEstimate.q.z;

		dq[0] = currentStateEstimate.dq.w;
		dq[1] = currentStateEstimate.dq.x;
		dq[2] = currentStateEstimate.dq.y;
		dq[3] = currentStateEstimate.dq.z;

        xy[0] = currentStateEstimate.position[0];
		xy[1] = currentStateEstimate.position[1];

        dxy[0] = currentStateEstimate.velocity[0]; // velocity in inertial frame
        dxy[1] = currentStateEstimate.velocity[1];

        velocityRef[0] = desiredVelocity_.x;
        velocityRef[1] = desiredVelocity_.y;

        desiredYaw += dt * desiredAngularYawVelocity_;

		Step(xy, q, dxy, dq, velocityRef, velocityRefGivenInHeadingFrame_, desiredYaw, params_.VelocityClamp, params_.AngularVelocityClamp, params_.AccelerationLimit, params_.MaxTilt, params_.IntegralEnabled, params_.PositionControlAtZeroVelocityReference, params_.PositionControlAtZeroVelocityReference_MaximumKickinVelocity, params_.StabilizationDetectionVelocity, dt, q_ref);

        quaternionMsg.w = q_ref[0];
        quaternionMsg.x = q_ref[1];
        quaternionMsg.y = q_ref[2];
        quaternionMsg.z = q_ref[3];

        quaternionPub_.publish(quaternionMsg);
        ros::spinOnce(); // walks the callback queue and calls registered callbacks for any outstanding events (incoming msgs, svc reqs, timers)
        loop_rate.sleep();
    }
}

void VelocityLQR::StateEstimateCallback(const kugle_msgs::StateEstimate::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Received odometry, translational velocity: (" << msg->twist.twist.linear.x << ", " << msg->twist.twist.linear.y << ")");
    currentStateEstimate.q = msg->q;
	currentStateEstimate.dq = msg->dq;
	currentStateEstimate.position = msg->position;
	currentStateEstimate.velocity = msg->velocity;
}

void VelocityLQR::VelocityReferenceCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    desiredVelocity_ = msg->linear;
    desiredAngularYawVelocity_ = msg->angular.z;
    velocityRefGivenInHeadingFrame_ = true;
    lastReferenceUpdateTime_ = ros::Time::now();
}

void VelocityLQR::VelocityReferenceInertialCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    desiredVelocity_ = msg->linear;
    desiredAngularYawVelocity_ = msg->angular.z;
    velocityRefGivenInHeadingFrame_ = false;
    lastReferenceUpdateTime_ = ros::Time::now();
}

void VelocityLQR::Step(const double xy[2], const double q[4], const double dxy[2], const double dq[4], const double velocityRef[2], const bool velocityRefGivenInHeadingFrame, const double headingRef, const double velocity_error_clamp, const double angular_velocity_clamp, const double acceleration_limit, const double MaxTilt, const bool VelocityIntegralEnabled, const bool PositionControlAtZeroVelocityReference, const double PositionControlAtZeroVelocityReference_MaximumKickinVelocity, const double StabilizationDetectionVelocity, const double dt, double q_ref_out[4])
{
	/* Form error state vector */
	// The error state is linearized in the heading frame. Hence both velocity error, quaternions etc. has to be transformed into heading frame (rotated with heading)
	// X_err = [dx_err_integral, dy_err_integral, q2, q3, dx_err, dy_err, dq2, dq3, q2_ref_prev, q3_ref_prev]
	Eigen::Matrix<double, 10, 1> X_err;
	double * pos_err_heading = &X_err(0);
	double * vel_err_integral_heading = &X_err(0);
	double * q_tilt_xy = &X_err(2);
	double * vel_err_heading = &X_err(4);
	double * dq_tilt_xy = &X_err(6);
	double * q_ref_tilt_xy = &X_err(8);

	/* Extract current heading */
	double heading = HeadingFromQuaternion(q);
	double q_heading[4];
	HeadingQuaternion(q, q_heading);

	/* Convert quaternion parts into heading frame */
	/* Compute previously set reference quaternion */
	double q_ref_tilt[4];
	Quaternion_eul2quat_zyx(0, pitch_ref, roll_ref, q_ref_tilt);

	/* Extract tilt quaternion */
	double q_tilt[4]; // current tilt
	/* q_tilt = q_heading* o q; */
	Quaternion_PhiT(q_heading, q, q_tilt);

	/* Extract body angular velocity */
	double omega_body[3];
	Quaternion_GetAngularVelocity_Body(q, dq, omega_body);

	/* Compute quaternion derivative in heading frame */
	double dq_tilt[4];
	/* dq_tilt = 1/2 * Phi(q_tilt) * [0;omega_body]; */
	Quaternion_GetDQ_FromBody(q_tilt, omega_body, dq_tilt);

	/* Set state vector with quaternion states */
	q_ref_tilt_xy[0] = q_ref_tilt[1];
	q_ref_tilt_xy[1] = q_ref_tilt[2];
	q_tilt_xy[0] = q_tilt[1];
	q_tilt_xy[1] = q_tilt[2];
	dq_tilt_xy[0] = dq_tilt[1];
	dq_tilt_xy[1] = dq_tilt[2];

	/* Update position reference if we are supposed to be moving */
	if (!position_control_enabled) {
		position_reference[0] = xy[0];
		position_reference[1] = xy[1];
	}

	/* Enable position control (station keeping) if velocity reference is zero */
	if (!VelocityIntegralAfterPowerup && PositionControlAtZeroVelocityReference && velocityRef[0] == 0 && velocityRef[1] == 0 && (dxy[0]*dxy[0] + dxy[1]*dxy[1] < PositionControlAtZeroVelocityReference_MaximumKickinVelocity*PositionControlAtZeroVelocityReference_MaximumKickinVelocity)) {
		position_control_enabled = true;
	} else {
		position_control_enabled = false;
	}

	if (VelocityIntegralInitializationTime > 0) {
		VelocityIntegralInitializationTime -= dt;
		if (VelocityIntegralInitializationTime < 0) VelocityIntegralInitializationTime = 0;

		if (velocityRef[0] != 0 || velocityRef[1] != 0) { // velocity reference demands movement
			VelocityIntegralInitializationTime = 0;
			VelocityIntegralAfterPowerup = false;
		}
	}
	else if (VelocityIntegralAfterPowerup) {
		if (velocityRef[0] != 0 || velocityRef[1] != 0 ||
			(dxy[0]*dxy[0] + dxy[1]*dxy[1] > StabilizationDetectionVelocity*StabilizationDetectionVelocity)) { // manual movement of velocity reference requires the initialization integral to be disabled
			VelocityIntegralInitializationTime = 0;
			VelocityIntegralAfterPowerup = false;
		}
	}

	/* Compute position error */
	double pos_err[2];
	pos_err[0] = xy[0] - position_reference[0];
	pos_err[1] = xy[1] - position_reference[1];

	/* Rotate position error from inertial frame into heading frame */
	Math_Rotate2D(pos_err, -heading, pos_err_heading);

	/* Compute velocity error */
	double vel_err[2];
	vel_err[0] = dxy[0];
	vel_err[1] = dxy[1];

	if (!velocityRefGivenInHeadingFrame) {
		vel_err[0] -= velocityRef[0];
		vel_err[1] -= velocityRef[1];
	}

	/* Rotation velocity error from inertial frame into heading frame */
	Math_Rotate2D(vel_err, -heading, vel_err_heading);

	if (velocityRefGivenInHeadingFrame) {
		vel_err_heading[0] -= velocityRef[0];
		vel_err_heading[1] -= velocityRef[1];
	}

	/* Compute reference in different frames for debugging */
	if (velocityRefGivenInHeadingFrame) {
		Velocity_Reference_Filtered_Heading[0] = velocityRef[0];
		Velocity_Reference_Filtered_Heading[1] = velocityRef[1];
		Math_Rotate2D(velocityRef, heading, Velocity_Reference_Filtered_Inertial);
	} else {
		Velocity_Reference_Filtered_Inertial[0] = velocityRef[0];
		Velocity_Reference_Filtered_Inertial[1] = velocityRef[1];
		Math_Rotate2D(velocityRef, -heading, Velocity_Reference_Filtered_Heading);
	}

	/* Clamp velocity error */
	vel_err_heading[0] = fminf(fmaxf(vel_err_heading[0], -velocity_error_clamp), velocity_error_clamp);
	vel_err_heading[1] = fminf(fmaxf(vel_err_heading[1], -velocity_error_clamp), velocity_error_clamp);

	/* Include integral term as current integral value */
	vel_err_integral_heading[0] += velocity_error_integral[0];
	vel_err_integral_heading[1] += velocity_error_integral[1];

	/* Update integral */
	if (VelocityIntegralEnabled || VelocityIntegralAfterPowerup) {
		// Rate limit the integration
		velocity_error_integral[0] += dt * fmaxf(fminf(vel_err_heading[0], 0.05), -0.05);
		velocity_error_integral[1] += dt * fmaxf(fminf(vel_err_heading[1], 0.05), -0.05);
	}

	/* Compute angular velocity reference by matrix multiplication with LQR gain */
	Eigen::Matrix<double, 2, 1> omega_ref_control;
	omega_ref_control = -gainMatrix * X_err;

	/* Saturate angular velocity output */
	omega_ref_control(0) = fminf(fmaxf(omega_ref_control(0), -angular_velocity_clamp), angular_velocity_clamp);
	omega_ref_control(1) = fminf(fmaxf(omega_ref_control(1), -angular_velocity_clamp), angular_velocity_clamp);

	/* Filter angular velocity before setting as output */
	omega_ref_control(0) = _omega_x_ref_filt.Filter(omega_ref_control(0));
	omega_ref_control(1) = _omega_y_ref_filt.Filter(omega_ref_control(1));

	/* Integrate angle reference */
	roll_ref += dt * omega_ref_control(0);
	pitch_ref += dt * omega_ref_control(1);

	/* Saturate roll and pitch references */
	roll_ref = fminf(fmaxf(roll_ref, -MaxTilt), MaxTilt);
	pitch_ref = fminf(fmaxf(pitch_ref, -MaxTilt), MaxTilt);

	/* Compute and set output quaternion */
	Quaternion_eul2quat_zyx(headingRef, pitch_ref, roll_ref, q_ref_out);
}

} // end namespace kugle_misc


#ifndef DQMC_CHECKERBOARD_HPP
#define DQMC_CHECKERBOARD_HPP

#include "la.hpp"
#include "parameters.hpp"

#include <boost/multi_array.hpp>

#include <exception>
#include <cmath>

namespace dqmc {
    namespace cx_checkerboard {

	inline void graph_to_checkerboard(dqmc::parameters& p,
					  dqmc::cx_workspace& ws) {
	    for (int i = 0; i < p.num_bonds; ++i) {
		ws.bond_used[i] = -1;
	    }

	    int bonds_used = 0;
	    int coupling_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2, bt;
	    double t;
	    while (bonds_used < p.num_bonds) {
		coupling_used = 0;
		while (coupling_used < p.ts.size()) {
		    cx_sp_mat hopping(p.N, p.N);
		    cx_sp_mat hopping_inv(p.N, p.N);

		    int elements = 0;

		    for (int i = 0; i < p.N; ++i) {
			ws.site_used[i] = -1;
		    }

		    for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
			if (p.graph.bond_type(*itr1) != coupling_used) continue;

			b = p.graph.index(*itr1);
			s1 = p.graph.source(*itr1);
			s2 = p.graph.target(*itr1);
			bt = p.graph.bond_type(*itr1);

			if (ws.bond_used[b] == 1) continue;
			if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

			ws.chkr_bonds[bt].push_back(bond(s1, s2));

			ws.bond_used[b] = 1;
			ws.site_used[s1] = 1;
			ws.site_used[s2] = 1;
			t = p.ts[bt];
			hopping.insert(s1, s1) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping.insert(s2, s2) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping.insert(s1, s2) = cx_double(sinh(t * p.delta_tau/2.), 0);
			hopping.insert(s2, s1) = cx_double(sinh(t * p.delta_tau/2.), 0);

			hopping_inv.insert(s1, s1) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping_inv.insert(s2, s2) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping_inv.insert(s1, s2) = cx_double(-sinh(t * p.delta_tau/2.), 0);
			hopping_inv.insert(s2, s1) = cx_double(-sinh(t * p.delta_tau/2.), 0);

			// std::cout << "Using " << s1 << " and " << s2 << std::endl;
			++bonds_used;
			++elements;
		    }

		    if (elements != 0) {
			for (int i = 0; i < p.N; ++i) {
			    if(ws.site_used[i] == -1) {
				hopping.insert(i, i) = 1.;
				hopping_inv.insert(i, i) = 1.;
			    }
			}
			// arma::mat(hopping).print("A hopping matrix");
			ws.sparse_hoppings.push_back(hopping);
			ws.sparse_hoppings_inv.push_back(hopping_inv);
		    }
		    ++coupling_used;
		}
	    }
	}

	inline void graph_to_checkerboard_renyi(dqmc::parameters& p, dqmc::cx_workspace& ws) {
	    for (int i = 0; i < p.num_bonds; ++i) {
		ws.bond_used[i] = -1;
	    }

	    int bonds_used = 0;
	    int coupling_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2, s1a, s2a;
	    double t;
	    while (bonds_used < p.num_bonds) {
		coupling_used = 0;
		while (coupling_used < p.ts.size()) {
		    cx_sp_mat hopping_0(ws.vol, ws.vol), hopping_0_inv(ws.vol, ws.vol),
			hopping_1(ws.vol, ws.vol), hopping_1_inv(ws.vol, ws.vol),
			hopping_2(ws.vol, ws.vol), hopping_2_inv(ws.vol, ws.vol),
			hopping_3(ws.vol, ws.vol), hopping_3_inv(ws.vol, ws.vol);

		    int elements = 0;

		    for (int i = 0; i < p.N; ++i) ws.site_used[i] = -1;
		    for (int i = 0; i < p.N + p.n_B; ++i) {
			hopping_0.insert(i, i) = cx_double(1., 0);
			hopping_0_inv.insert(i, i) = cx_double(1., 0);
			hopping_1.insert(i, i) =  cx_double(1., 0);
			hopping_1_inv.insert(i, i) =  cx_double(1., 0);
			hopping_2.insert(i, i) =  cx_double(1., 0);
			hopping_2_inv.insert(i, i) =  cx_double(1., 0);
			hopping_3.insert(i, i) =  cx_double(1., 0);
			hopping_3_inv.insert(i, i) =  cx_double(1., 0);
		    }

		    for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
			if (p.graph.bond_type(*itr1) != coupling_used) continue;

			b = p.graph.index(*itr1);
			s1 = p.graph.source(*itr1);
			s2 = p.graph.target(*itr1);
			s1a = s1;
			s2a = s2;

			if (ws.bond_used[b] == 1) continue;
			if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

			ws.bond_used[b] = 1;
			ws.site_used[s1] = 1;
			ws.site_used[s2] = 1;

			t = p.ts[p.graph.bond_type(*itr1)];

			if (s1 >= p.n_A) s1a = s1 + p.n_B;
			if (s2 >= p.n_A) s2a = s2 + p.n_B;

			hopping_0.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0.coeffRef(s1, s2) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_0.coeffRef(s2, s1) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_0_inv.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0_inv.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0_inv.coeffRef(s1, s2) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_0_inv.coeffRef(s2, s1) = cx_double(-sinh(t * p.delta_tau/2.));

			hopping_1.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1.coeffRef(s1, s2) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_1.coeffRef(s2, s1) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_1_inv.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1_inv.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1_inv.coeffRef(s1, s2) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_1_inv.coeffRef(s2, s1) = cx_double(-sinh(t * p.delta_tau/2.));

			hopping_2.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2.coeffRef(s1a, s2a) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_2.coeffRef(s2a, s1a) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_2_inv.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2_inv.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2_inv.coeffRef(s1a, s2a) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_2_inv.coeffRef(s2a, s1a) = cx_double(-sinh(t * p.delta_tau/2.));

			hopping_3.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3.coeffRef(s1a, s2a) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_3.coeffRef(s2a, s1a) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_3_inv.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3_inv.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3_inv.coeffRef(s1a, s2a) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_3_inv.coeffRef(s2a, s1a) = cx_double(-sinh(t * p.delta_tau/2.));

			// std::cout << "Using " << s1 << " and " << s2 << std::endl;
			++bonds_used;
			++elements;
		    }

		    if (elements != 0) {
			// arma::mat(hopping_0).print("Pushing back");
			using namespace std;
			ws.sparse_hoppings_0.push_back(hopping_0);
			ws.sparse_hoppings_0_inv.push_back(hopping_0_inv);

			ws.sparse_hoppings_1.push_back(hopping_1);
			ws.sparse_hoppings_1_inv.push_back(hopping_1_inv);

			ws.sparse_hoppings_2.push_back(hopping_2);
			ws.sparse_hoppings_2_inv.push_back(hopping_2_inv);

			ws.sparse_hoppings_3.push_back(hopping_3);
			ws.sparse_hoppings_3_inv.push_back(hopping_3_inv);
		    }
		    ++coupling_used;
		}
	    }
	}

	inline void hop_left(dqmc::cx_workspace * ws, cx_mat& M, double pref) {
	    using namespace std;
	    int par = 0;

	    if (pref > 0) {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings[i] * M;
		    else
			M = ws->sparse_hoppings[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings[i] * M;
		    else
			M = ws->sparse_hoppings[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    else {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
		    else
			M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
		    else
			M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_right(dqmc::cx_workspace * ws, cx_mat& M, double pref) {
	    int par = 0;

	    if (pref > 0) {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings[i];
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings[i];
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    else {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings_inv[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings_inv[i];
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings_inv[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings_inv[i];
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	// inline void hop_left_renyi(dqmc::cx_parameters * p,
	// 			   dqmc::cx_workspace * ws,
	// 			   cx_mat& M, double spin,
	// 			   double pref, int section,
	// 			   int slice) {
	//     using namespace std;
	//     int par = 0;
	//     // cout << "section " << section << endl;
	//     if (pref > 0) {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	// dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     else {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	// dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     if (par == 1)
	// 	M = ws->hop_temp;
	// }

	// inline void hop_right_renyi(dqmc::cx_parameters * p,
	// 			    dqmc::cx_workspace * ws,
	// 			    cx_mat&__restrict__ M, double spin,
	// 			    double pref, int section,
	// 			    int slice) {
	//     int par = 0;

	//     if (pref > 0) {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     else {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     if (par == 1)
	// 	M = ws->hop_temp;
	// }
    }
}
#endif


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/models/cdsoptionhelper.hpp>
#include <qle/pricingengines/blackcdsoptionengine.hpp>
#include <qle/pricingengines/midpointcdsengine.hpp>

#include <ql/exercise.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/time/schedule.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

CdsOptionHelper::CdsOptionHelper(const Date& exerciseDate, const Handle<Quote>& volatility, const Protection::Side side,
                                 const Schedule& schedule, const BusinessDayConvention paymentConvention,
                                 const DayCounter& dayCounter,
                                 const Handle<DefaultProbabilityTermStructure>& probability, const Real recoveryRate,
                                 const Handle<YieldTermStructure>& termStructure, const Rate spread, const Rate upfront,
                                 const bool settlesAccrual, const bool paysAtDefaultTime, const Date protectionStart,
                                 const Date upfrontDate, const boost::shared_ptr<Claim>& claim,
                                 const BlackCalibrationHelper::CalibrationErrorType errorType)
    : BlackCalibrationHelper(volatility, termStructure, errorType), blackVol_(boost::make_shared<SimpleQuote>(0.0)) {

    boost::shared_ptr<PricingEngine> cdsEngine =
        boost::make_shared<QuantExt::MidPointCdsEngine>(probability, recoveryRate, termStructure);

    boost::shared_ptr<CreditDefaultSwap> tmp;
    if (upfront == Null<Real>())
        tmp = boost::shared_ptr<CreditDefaultSwap>(new CreditDefaultSwap(side, 1.0, 0.02, schedule, paymentConvention,
                                                                         dayCounter, settlesAccrual, paysAtDefaultTime,
                                                                         protectionStart, claim));
    else
        tmp = boost::shared_ptr<CreditDefaultSwap>(
            new CreditDefaultSwap(side, 1.0, upfront, 0.02, schedule, paymentConvention, dayCounter, settlesAccrual,
                                  paysAtDefaultTime, protectionStart, upfrontDate, claim));
    tmp->setPricingEngine(cdsEngine);

    Real strike = spread == Null<Real>() ? tmp->fairSpread() : spread;
    if (upfront == Null<Real>())
        cds_ = boost::shared_ptr<CreditDefaultSwap>(new CreditDefaultSwap(side, 1.0, strike, schedule,
                                                                          paymentConvention, dayCounter, settlesAccrual,
                                                                          paysAtDefaultTime, protectionStart, claim));
    else
        cds_ = boost::shared_ptr<CreditDefaultSwap>(
            new CreditDefaultSwap(side, 1.0, upfront, strike, schedule, paymentConvention, dayCounter, settlesAccrual,
                                  paysAtDefaultTime, protectionStart, upfrontDate, claim));

    cds_->setPricingEngine(cdsEngine);

    boost::shared_ptr<Exercise> exercise = boost::make_shared<EuropeanExercise>(exerciseDate);

    option_ = boost::make_shared<CdsOption>(cds_, exercise, true);
    Handle<BlackVolTermStructure> h(
        boost::make_shared<BlackConstantVol>(0, NullCalendar(), Handle<Quote>(blackVol_), Actual365Fixed()));

    blackEngine_ = boost::make_shared<BlackCdsOptionEngine>(probability, recoveryRate, termStructure, h);
}

Real CdsOptionHelper::modelValue() const {
    calculate();
    option_->setPricingEngine(engine_);
    return option_->NPV();
}

Real CdsOptionHelper::blackPrice(Volatility sigma) const {
    calculate();
    blackVol_->setValue(sigma);
    option_->setPricingEngine(blackEngine_);
    Real value = option_->NPV();
    option_->setPricingEngine(engine_);
    return value;
}

} // namespace QuantExt


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015, 2017.
// Modifications copyright (c) 2015-2017, Oracle and/or its affiliates.

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_SECTIONS_FUNCTIONS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_SECTIONS_FUNCTIONS_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/algorithms/detail/recalculate.hpp>
#include <boost/geometry/policies/robustness/robust_point_type.hpp>

// For spherical/geographic longitudes covered_by point/box
#include <boost/geometry/strategies/cartesian/point_in_box.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace section
{

template
<
    std::size_t Dimension,
    typename Geometry,
    typename CastedCSTag = typename tag_cast
                            <
                                typename cs_tag<Geometry>::type,
                                spherical_tag
                            >::type
>
struct preceding_check
{
    template <typename Point, typename Box>
    static inline bool apply(int dir, Point const& point, Box const& /*point_box*/, Box const& other_box)
    {
        return (dir == 1  && get<Dimension>(point) < get<min_corner, Dimension>(other_box))
            || (dir == -1 && get<Dimension>(point) > get<max_corner, Dimension>(other_box));
    }
};

template <typename Geometry>
struct preceding_check<0, Geometry, spherical_tag>
{
    template <typename Point, typename Box>
    static inline bool apply(int dir, Point const& point, Box const& point_box, Box const& other_box)
    {
        typedef typename select_coordinate_type
            <
                Point, Box
            >::type calc_t;
        typedef typename coordinate_system<Point>::type::units units_t;

        calc_t const c0 = 0;

        calc_t const value = get<0>(point);
        calc_t const other_min = get<min_corner, 0>(other_box);
        calc_t const other_max = get<max_corner, 0>(other_box);

        bool const pt_covered = strategy::within::covered_by_range
                                    <
                                        Point, 0, spherical_tag
                                    >::apply(value,
                                             other_min,
                                             other_max);

        if (pt_covered)
        {
            return false;
        }

        if (dir == 1)
        {
            calc_t const diff_min = math::longitude_distance_signed
                                        <
                                            units_t, calc_t
                                        >(other_min, value);

            calc_t const diff_min_min = math::longitude_distance_signed
                                        <
                                            units_t, calc_t
                                        >(other_min, get<min_corner, 0>(point_box));

            return diff_min < c0 && diff_min_min <= c0 && diff_min_min <= diff_min;
        }
        else if (dir == -1)
        {
            calc_t const diff_max = math::longitude_distance_signed
                                        <
                                            units_t, calc_t
                                        >(other_max, value);

            calc_t const diff_max_max = math::longitude_distance_signed
                                        <
                                            units_t, calc_t
                                        >(other_max, get<max_corner, 0>(point_box));

            return diff_max > c0 && diff_max_max >= c0 && diff_max <= diff_max_max;
        }

        return false;
    }
};

template
<
    std::size_t Dimension,
    typename Point,
    typename RobustBox,
    typename RobustPolicy
>
static inline bool preceding(int dir,
                             Point const& point,
                             RobustBox const& point_robust_box,
                             RobustBox const& other_robust_box,
                             RobustPolicy const& robust_policy)
{
    typename geometry::robust_point_type<Point, RobustPolicy>::type robust_point;
    geometry::recalculate(robust_point, point, robust_policy);
    return preceding_check<Dimension, Point>::apply(dir, robust_point, point_robust_box, other_robust_box);
}

template
<
    std::size_t Dimension,
    typename Point,
    typename RobustBox,
    typename RobustPolicy
>
static inline bool exceeding(int dir,
                             Point const& point,
                             RobustBox const& point_robust_box,
                             RobustBox const& other_robust_box,
                             RobustPolicy const& robust_policy)
{
    return preceding<Dimension>(-dir, point, point_robust_box, other_robust_box, robust_policy);
}

}} // namespace detail::section
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_SECTIONS_FUNCTIONS_HPP


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

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <cuda.h>
#include <cuda_runtime_api.h>
#include <cudnn.h>
#include <getopt.h>
#include <iostream>
#include <string>
#include <unsupported/Eigen/CXX11/Tensor>
#include <vector>

#include "../examples/device_ptr.h"
#include "cudnn_wrappers.h"
#include "haste.h"

using haste::v0::lstm::BackwardPass;
using haste::v0::lstm::ForwardPass;
using std::string;

using Tensor1 = Eigen::Tensor<float, 1>;
using Tensor2 = Eigen::Tensor<float, 2>;
using Tensor3 = Eigen::Tensor<float, 3>;

static constexpr int DEFAULT_SAMPLE_SIZE = 10;
static constexpr int DEFAULT_TIME_STEPS = 50;

static cudnnHandle_t g_cudnn_handle;
static cublasHandle_t g_blas_handle;

float TimeLoop(std::function<void()> fn, int iterations) {
  cudaEvent_t start, stop;
  cudaEventCreate(&start);
  cudaEventCreate(&stop);
  cudaEventRecord(start);
  for (int i = 0; i < iterations; ++i)
    fn();
  float elapsed_ms;
  cudaEventRecord(stop);
  cudaEventSynchronize(stop);
  cudaEventElapsedTime(&elapsed_ms, start, stop);
  cudaEventDestroy(start);
  cudaEventDestroy(stop);
  return elapsed_ms / iterations;
}

float CudnnInference(
    int sample_size,
    const Tensor2& W,
    const Tensor2& R,
    const Tensor1& b,
    const Tensor3& x) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  device_ptr<Tensor3> x_dev(x);

  device_ptr<Tensor2> h_dev(batch_size * hidden_size);
  device_ptr<Tensor2> c_dev(batch_size * hidden_size);
  device_ptr<Tensor3> y_dev(time_steps * batch_size * hidden_size);
  device_ptr<Tensor2> h_out_dev(batch_size * hidden_size);
  device_ptr<Tensor2> c_out_dev(batch_size * hidden_size);

  h_dev.zero();
  c_dev.zero();

  // Descriptors all the way down. Nice.
  RnnDescriptor<float> rnn_descriptor(g_cudnn_handle, hidden_size, CUDNN_LSTM);

  TensorDescriptorArray<float> x_descriptors(time_steps, { batch_size, input_size, 1 });
  TensorDescriptorArray<float> y_descriptors(time_steps, { batch_size, hidden_size, 1 });

  auto h_descriptor = TensorDescriptor<float>({ 1, batch_size, hidden_size });
  auto c_descriptor = TensorDescriptor<float>({ 1, batch_size, hidden_size });
  auto h_out_descriptor = TensorDescriptor<float>({ 1, batch_size, hidden_size });
  auto c_out_descriptor = TensorDescriptor<float>({ 1, batch_size, hidden_size });

  size_t workspace_size;
  cudnnGetRNNWorkspaceSize(
      g_cudnn_handle,
      *rnn_descriptor,
      time_steps,
      &x_descriptors,
      &workspace_size);
  auto workspace_dev = device_ptr<Tensor1>::NewByteSized(workspace_size);

  size_t w_count;
  cudnnGetRNNParamsSize(
      g_cudnn_handle,
      *rnn_descriptor,
      *&x_descriptors,
      &w_count,
      CUDNN_DATA_FLOAT);

  auto w_dev = device_ptr<Tensor1>::NewByteSized(w_count);
  FilterDescriptor<float> w_descriptor(w_dev.Size());

  float ms = TimeLoop([&]() {
    cudnnRNNForwardInference(
        g_cudnn_handle,
        *rnn_descriptor,
        time_steps,
        &x_descriptors,
        x_dev.data,
        *h_descriptor,
        h_dev.data,
        *c_descriptor,
        c_dev.data,
        *w_descriptor,
        w_dev.data,
        &y_descriptors,
        y_dev.data,
        *h_out_descriptor,
        h_out_dev.data,
        *c_out_descriptor,
        c_out_dev.data,
        workspace_dev.data,
        workspace_size);
  }, sample_size);
  return ms;
}

float CudnnTrain(
    int sample_size,
    const Tensor2& W,
    const Tensor2& R,
    const Tensor1& b,
    const Tensor3& x,
    const Tensor3& dh,
    const Tensor3& dc) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  device_ptr<Tensor3> y_dev(time_steps * batch_size * hidden_size);
  device_ptr<Tensor3> dy_dev(time_steps * batch_size * hidden_size);
  device_ptr<Tensor2> dhy_dev(batch_size * hidden_size);
  device_ptr<Tensor2> dcy_dev(batch_size * hidden_size);
  device_ptr<Tensor2> hx_dev(batch_size * hidden_size);
  device_ptr<Tensor2> cx_dev(batch_size * hidden_size);
  device_ptr<Tensor2> dx_dev(time_steps * batch_size * input_size);
  device_ptr<Tensor2> dhx_dev(batch_size * hidden_size);
  device_ptr<Tensor2> dcx_dev(batch_size * hidden_size);

  RnnDescriptor<float> rnn_descriptor(g_cudnn_handle, hidden_size, CUDNN_LSTM);
  TensorDescriptorArray<float> y_descriptors(time_steps, { batch_size, hidden_size, 1 });
  TensorDescriptorArray<float> dy_descriptors(time_steps, { batch_size, hidden_size, 1 });
  TensorDescriptorArray<float> dx_descriptors(time_steps, { batch_size, input_size, 1 });

  TensorDescriptor<float> dhy_descriptor({ 1, batch_size, hidden_size });
  TensorDescriptor<float> dcy_descriptor({ 1, batch_size, hidden_size });
  TensorDescriptor<float> hx_descriptor({ 1, batch_size, hidden_size });
  TensorDescriptor<float> cx_descriptor({ 1, batch_size, hidden_size });
  TensorDescriptor<float> dhx_descriptor({ 1, batch_size, hidden_size });
  TensorDescriptor<float> dcx_descriptor({ 1, batch_size, hidden_size });

  size_t workspace_size = 0;
  cudnnGetRNNWorkspaceSize(
      g_cudnn_handle,
      *rnn_descriptor,
      time_steps,
      &dx_descriptors,
      &workspace_size);
  auto workspace_dev = device_ptr<Tensor1>::NewByteSized(workspace_size);

  size_t w_count = 0;
  cudnnGetRNNParamsSize(
      g_cudnn_handle,
      *rnn_descriptor,
      *&dx_descriptors,
      &w_count,
      CUDNN_DATA_FLOAT);

  auto w_dev = device_ptr<Tensor1>::NewByteSized(w_count);
  FilterDescriptor<float> w_descriptor(w_dev.Size());

  size_t reserve_size = 0;
  cudnnGetRNNTrainingReserveSize(
      g_cudnn_handle,
      *rnn_descriptor,
      time_steps,
      &dx_descriptors,
      &reserve_size);
  auto reserve_dev = device_ptr<Tensor1>::NewByteSized(reserve_size);

  float ms = TimeLoop([&]() {
    cudnnRNNForwardTraining(
        g_cudnn_handle,
        *rnn_descriptor,
        time_steps,
        &dx_descriptors,
        dx_dev.data,
        *hx_descriptor,
        hx_dev.data,
        *cx_descriptor,
        cx_dev.data,
        *w_descriptor,
        w_dev.data,
        &y_descriptors,
        y_dev.data,
        *dhy_descriptor,
        dhy_dev.data,
        *dcy_descriptor,
        dcy_dev.data,
        workspace_dev.data,
        workspace_size,
        reserve_dev.data,
        reserve_size);

    cudnnRNNBackwardData(
        g_cudnn_handle,
        *rnn_descriptor,
        time_steps,
        &y_descriptors,
        y_dev.data,
        &dy_descriptors,
        dy_dev.data,
        *dhy_descriptor,
        dhy_dev.data,
        *dcy_descriptor,
        dcy_dev.data,
        *w_descriptor,
        w_dev.data,
        *hx_descriptor,
        hx_dev.data,
        *cx_descriptor,
        cx_dev.data,
        &dx_descriptors,
        dx_dev.data,
        *dhx_descriptor,
        dhx_dev.data,
        *dcx_descriptor,
        dcx_dev.data,
        workspace_dev.data,
        workspace_size,
        reserve_dev.data,
        reserve_size);

    cudnnRNNBackwardWeights(
        g_cudnn_handle,
        *rnn_descriptor,
        time_steps,
        &dx_descriptors,
        dx_dev.data,
        *hx_descriptor,
        hx_dev.data,
        &y_descriptors,
        y_dev.data,
        workspace_dev.data,
        workspace_size,
        *w_descriptor,
        w_dev.data,
        reserve_dev.data,
        reserve_size);
  }, sample_size);
  return ms;
}

float HasteInference(
    int sample_size,
    const Tensor2& W,
    const Tensor2& R,
    const Tensor1& b,
    const Tensor3& x) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  // Copy weights over to GPU.
  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor1> b_dev(b);
  device_ptr<Tensor3> x_dev(x);

  device_ptr<Tensor3> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> c_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> v_dev(time_steps * batch_size * hidden_size * 4);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 4);

  h_dev.zero();
  c_dev.zero();

  // Settle down the GPU and off we go!
  cudaDeviceSynchronize();
  float ms = TimeLoop([&]() {
    ForwardPass<float> forward(
        false,
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    forward.Run(
        time_steps,
        W_dev.data,
        R_dev.data,
        b_dev.data,
        x_dev.data,
        h_dev.data,
        c_dev.data,
        v_dev.data,
        tmp_Rh_dev.data,
        0.0f,
        nullptr);
  }, sample_size);
  return ms;
}

float HasteTrain(
    int sample_size,
    const Tensor2& W,
    const Tensor2& R,
    const Tensor1& b,
    const Tensor3& x,
    const Tensor3& dh,
    const Tensor3& dc) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  Eigen::array<int, 3> transpose_x({ 1, 2, 0 });
  Tensor3 x_t = x.shuffle(transpose_x);

  Eigen::array<int, 2> transpose({ 1, 0 });
  Tensor2 W_t = W.shuffle(transpose);
  Tensor2 R_t = R.shuffle(transpose);

  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor3> x_dev(x);
  device_ptr<Tensor3> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> c_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> v_dev(time_steps * batch_size * hidden_size * 4);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 4);

  device_ptr<Tensor2> W_t_dev(W_t);
  device_ptr<Tensor2> R_t_dev(R_t);
  device_ptr<Tensor1> b_dev(b);
  device_ptr<Tensor3> x_t_dev(x_t);

  // These gradients should actually come "from above" but we're just allocating
  // a bunch of uninitialized memory and passing it in.
  device_ptr<Tensor3> dh_new_dev(dh);
  device_ptr<Tensor3> dc_new_dev(dc);

  device_ptr<Tensor3> dx_dev(time_steps * batch_size * input_size);
  device_ptr<Tensor2> dW_dev(input_size * hidden_size * 4);
  device_ptr<Tensor2> dR_dev(hidden_size * hidden_size * 4);
  device_ptr<Tensor2> db_dev(hidden_size * 4);
  device_ptr<Tensor2> dh_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor2> dc_dev((time_steps + 1) * batch_size * hidden_size);

  dW_dev.zero();
  dR_dev.zero();
  db_dev.zero();
  dh_dev.zero();
  dc_dev.zero();

  ForwardPass<float> forward(
      true,
      batch_size,
      input_size,
      hidden_size,
      g_blas_handle);

  BackwardPass<float> backward(
      batch_size,
      input_size,
      hidden_size,
      g_blas_handle);

  static const float alpha = 1.0f;
  static const float beta = 0.0f;

  cudaDeviceSynchronize();
  float ms = TimeLoop([&]() {
    forward.Run(
        time_steps,
        W_dev.data,
        R_dev.data,
        b_dev.data,
        x_dev.data,
        h_dev.data,
        c_dev.data,
        v_dev.data,
        tmp_Rh_dev.data,
        0.0f,
        nullptr);

    // Haste needs `x`, `W`, and `R` to be transposed between the forward
    // pass and backward pass. Add these transposes in here to get a fair
    // measurement of the overall time it takes to run an entire training
    // loop.
    cublasSgeam(
        g_blas_handle,
        CUBLAS_OP_T, CUBLAS_OP_N,
        batch_size * time_steps, input_size,
        &alpha,
        x_dev.data, input_size,
        &beta,
        x_dev.data, batch_size * time_steps,
        x_t_dev.data, batch_size * time_steps);

    cublasSgeam(
        g_blas_handle,
        CUBLAS_OP_T, CUBLAS_OP_N,
        input_size, hidden_size * 4,
        &alpha,
        W_dev.data, hidden_size * 4,
        &beta,
        W_dev.data, input_size,
        W_t_dev.data, input_size);

    cublasSgeam(
        g_blas_handle,
        CUBLAS_OP_T, CUBLAS_OP_N,
        hidden_size, hidden_size * 4,
        &alpha,
        R_dev.data, hidden_size * 4,
        &beta,
        R_dev.data, hidden_size,
        R_t_dev.data, hidden_size);

    backward.Run(
        time_steps,
        W_t_dev.data,
        R_t_dev.data,
        b_dev.data,
        x_t_dev.data,
        h_dev.data,
        c_dev.data,
        dh_new_dev.data,
        dc_new_dev.data,
        dx_dev.data,
        dW_dev.data,
        dR_dev.data,
        db_dev.data,
        dh_dev.data,
        dc_dev.data,
        v_dev.data,
        nullptr);
  }, sample_size);
  return ms;
}

void usage(const char* name) {
  printf("Usage: %s [OPTION]...\n", name);
  printf("  -h, --help\n");
  printf("  -i, --implementation IMPL <haste|cudnn> (default: haste)\n");
  printf("  -m, --mode MODE           <inference|training> (default: training)\n");
  printf("  -s, --sample_size NUM     number of runs to average over (default: %d)\n",
      DEFAULT_SAMPLE_SIZE);
  printf("  -t, --time_steps NUM      number of time steps in RNN (default: %d)\n",
      DEFAULT_TIME_STEPS);
}

int main(int argc, char* const* argv) {
  srand(time(0));

  cudnnCreate(&g_cudnn_handle);
  cublasCreate(&g_blas_handle);

  static struct option long_options[] = {
    { "help", no_argument, 0, 'h' },
    { "implementation", required_argument, 0, 'i' },
    { "mode", required_argument, 0, 'm' },
    { "sample_size", required_argument, 0, 's' },
    { "time_steps", required_argument, 0, 't' },
    { 0, 0, 0, 0 }
  };

  int c;
  int opt_index;
  bool inference_flag = false;
  bool haste_flag = true;
  int sample_size = DEFAULT_SAMPLE_SIZE;
  int time_steps = DEFAULT_TIME_STEPS;
  while ((c = getopt_long(argc, argv, "hi:m:s:t:", long_options, &opt_index)) != -1)
    switch (c) {
      case 'h':
        usage(argv[0]);
        return 0;
      case 'i':
        if (optarg[0] == 'c' || optarg[0] == 'C')
          haste_flag = false;
        break;
      case 'm':
        if (optarg[0] == 'i' || optarg[0] == 'I')
          inference_flag = true;
        break;
      case 's':
        sscanf(optarg, "%d", &sample_size);
        break;
      case 't':
        sscanf(optarg, "%d", &time_steps);
        break;
    }

  printf("# Benchmark configuration:\n");
  printf("#   Mode: %s\n", inference_flag ? "inference" : "training");
  printf("#   Implementation: %s\n", haste_flag ? "Haste" : "cuDNN");
  printf("#   Sample size: %d\n", sample_size);
  printf("#   Time steps: %d\n", time_steps);
  printf("#\n");
  printf("# batch_size,hidden_size,input_size,time_ms\n");

  for (const int N : { 1, 16, 32, 64, 128 }) {
    for (const int H : { 128, 256, 512, 768, 1024, 1536, 2048, 3072, 4096 }) {
      for (const int C : { 64, 128, 256, 512 }) {
        Tensor2 W(H * 4, C);
        Tensor2 R(H * 4, H);
        Tensor1 b(H * 4);
        Tensor3 x(C, N, time_steps);
        Tensor3 dh(H, N, time_steps + 1);
        Tensor3 dc(H, N, time_steps + 1);

        float ms;
        if (inference_flag) {
          if (haste_flag)
            ms = HasteInference(sample_size, W, R, b, x);
          else
            ms = CudnnInference(sample_size, W, R, b, x);
        } else {
          if (haste_flag)
            ms = HasteTrain(sample_size, W, R, b, x, dh, dc);
          else
            ms = CudnnTrain(sample_size, W, R, b, x, dh, dc);
        }
        printf("%d,%d,%d,%f\n", N, H, C, ms);
      }
    }
  }

  cublasDestroy(g_blas_handle);
  cudnnDestroy(g_cudnn_handle);
  return 0;
}