/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <iostream>
#include <random>

#include <boost/math/constants/constants.hpp>

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/valuecalculator.h>
#include <tomographer/densedm/tspacefigofmerit.h>

// -----------------------------------------------------------------------------
// fixture(s)

class const_value_calculator {
private:
  const int _constval;
public:
  const_value_calculator(int constval_) : _constval(constval_) { }

  typedef int ValueType;
  inline ValueType getValue(const int = 0) const { return _constval; }
};

class const_value_calculator_noconstcall {
private:
  const int _constval;
public:
  const_value_calculator_noconstcall(int constval_) : _constval(constval_) { }

  typedef int ValueType;
  inline ValueType getValue(const int = 0) { return _constval; }
};

class value_calculator_iscallconst {
public:
  value_calculator_iscallconst() { }

  typedef int ValueType;
  inline ValueType getValue(const int = 0) const { return true; }
  inline ValueType getValue(const int = 0) { return false; }
};

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_valuecalculator)

BOOST_AUTO_TEST_SUITE(multiplexorValueCalculator)

BOOST_AUTO_TEST_CASE(essential)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;

  DMTypes dmt;
  DMTypes::MatrixType rho_ref(dmt.initMatrixType());
  rho_ref <<
    1, 0,
    0, 0;
  DMTypes::MatrixType T_ref(dmt.initMatrixType());
  T_ref <<
    1, 0,
    0, 0;
  DMTypes::MatrixType A(dmt.initMatrixType());
  A <<
    2, 0,
    0, 1;
  DMTypes::MatrixType rho(dmt.initMatrixType());
  rho <<
    0.5, 0,
    0, 0.5;
  DMTypes::MatrixType T(dmt.initMatrixType());
  T <<
    boost::math::constants::half_root_two<double>(), 0,
    0, boost::math::constants::half_root_two<double>();

  const double correct_values[] = {
    // using rho_ref as ref state
    boost::math::constants::half_root_two<double>(),
    boost::math::constants::half_root_two<double>(),
    0.5,
    1.5,
    // using T*T' as ref state
    1,
    0,
    0,
    1.5
  };

  Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes> obs(dmt, A);

  for (int val = 0; val < 8; ++val) {
    Tomographer::MultiplexorValueCalculator<
      double,
      Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::PurifDistToRefCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::TrDistToRefCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::PurifDistToRefCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::TrDistToRefCalculator<DMTypes>,
      Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes>
      >  multiplexor(
	  val,
	  [&](){ return new Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes>(T_ref); },
	  [&](){ return new Tomographer::DenseDM::TSpace::PurifDistToRefCalculator<DMTypes>(T_ref); },
	  [&](){ return new Tomographer::DenseDM::TSpace::TrDistToRefCalculator<DMTypes>(rho_ref); },
	  [&](){ return new Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes>(obs); },
	  [&](){ return new Tomographer::DenseDM::TSpace::FidelityToRefCalculator<DMTypes>(T); },
	  [&](){ return new Tomographer::DenseDM::TSpace::PurifDistToRefCalculator<DMTypes>(T); },
	  [&](){ return new Tomographer::DenseDM::TSpace::TrDistToRefCalculator<DMTypes>(rho); },
	  [&](){ return new Tomographer::DenseDM::TSpace::ObservableValueCalculator<DMTypes>(obs); }
	  );
    MY_BOOST_CHECK_FLOATS_EQUAL(multiplexor.getValue(T), correct_values[val], tol);
  }
}

BOOST_AUTO_TEST_CASE(constnoconst)
{
  for (int val = 0; val < 2; ++val) {
    Tomographer::MultiplexorValueCalculator<
      int,
      const_value_calculator,
      const_value_calculator_noconstcall
      > multiplexor(
	  val,
	  []() { return new const_value_calculator(0); },
	  []() { return new const_value_calculator_noconstcall(1); }
	  );
    BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
  }
}

BOOST_AUTO_TEST_CASE(iscallconst)
{
  for (int val = 0; val < 2; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        value_calculator_iscallconst,
        value_calculator_iscallconst
        > multiplexor(
            val,
            []() { return new value_calculator_iscallconst(); },
            []() { return new value_calculator_iscallconst(); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), false);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        value_calculator_iscallconst,
        value_calculator_iscallconst
        > cmultiplexor(
            val,
            []() { return new value_calculator_iscallconst(); },
            []() { return new value_calculator_iscallconst(); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), true);
    }
  }
}

BOOST_AUTO_TEST_CASE(fixed1)
{
  {
    Tomographer::MultiplexorValueCalculator<
      int,
      const_value_calculator
      > multiplexor(0, [](){ return new const_value_calculator(191); });
    BOOST_CHECK_EQUAL(multiplexor.getValue(-1), 191);
  }
  {
    const Tomographer::MultiplexorValueCalculator<
      int,
      const_value_calculator
      > cmultiplexor(0, [](){ return new const_value_calculator(191); });
    BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), 191);
  }
}
BOOST_AUTO_TEST_CASE(fixed2)
{
  for (int val = 0; val < 2; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}
BOOST_AUTO_TEST_CASE(fixed3)
{
  for (int val = 0; val < 3; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}
BOOST_AUTO_TEST_CASE(fixed4)
{
  for (int val = 0; val < 4; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);    }
  }
}
BOOST_AUTO_TEST_CASE(fixed5)
{
  for (int val = 0; val < 5; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}
BOOST_AUTO_TEST_CASE(fixed6)
{
  for (int val = 0; val < 6; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}
BOOST_AUTO_TEST_CASE(fixed7)
{
  for (int val = 0; val < 7; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);    }
  }
}
BOOST_AUTO_TEST_CASE(fixed8)
{
  for (int val = 0; val < 8; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); },
            []() { return new const_value_calculator(7); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); },
            []() { return new const_value_calculator(7); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}
BOOST_AUTO_TEST_CASE(fixed9)
{
  for (int val = 0; val < 9; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); },
            []() { return new const_value_calculator(7); },
            []() { return new const_value_calculator(8); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); },
            []() { return new const_value_calculator(7); },
            []() { return new const_value_calculator(8); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}
BOOST_AUTO_TEST_CASE(fixed10)
{
  for (int val = 0; val < 10; ++val) {
    {
      Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > multiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); },
            []() { return new const_value_calculator(7); },
            []() { return new const_value_calculator(8); },
            []() { return new const_value_calculator(9); }
            );
      BOOST_CHECK_EQUAL(multiplexor.getValue(-1), val);
    }
    {
      const Tomographer::MultiplexorValueCalculator<
        int,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator,
        const_value_calculator
        > cmultiplexor(
            val,
            []() { return new const_value_calculator(0); },
            []() { return new const_value_calculator(1); },
            []() { return new const_value_calculator(2); },
            []() { return new const_value_calculator(3); },
            []() { return new const_value_calculator(4); },
            []() { return new const_value_calculator(5); },
            []() { return new const_value_calculator(6); },
            []() { return new const_value_calculator(7); },
            []() { return new const_value_calculator(8); },
            []() { return new const_value_calculator(9); }
            );
      BOOST_CHECK_EQUAL(cmultiplexor.getValue(-1), val);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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_IS_NEGATIVE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NEGATIVE_HPP_INCLUDED

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

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is negative else @ref False.

    This function differs from @ref is_ltz for floating point arguments,
    because @ref Mzero is negative but not less than zero, and @ref Mzero is
    not positive and not greater than zero, It's probably @ref is_ltz that
    you want.

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

    @par Note

     Using `is_negative(x)` is similar to `bitofsign(x) == 1`

    @par Example:

      @snippet is_negative.cpp is_negative

    @par Possible output:

      @snippet is_negative.txt is_negative

  **/
  as_logical_t<Value> is_negative(Value const& x);
} }
#endif

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

#endif


/*
 * Copyright 2014-2017 Jouni Roivas
 */

#include "blieng/point.h"
#include "blieng.h"

#include <boost/format.hpp>
#include <math.h>

#include <string>

using blieng::Point;

Point::~Point()
{
    x = 0;
    y = 0;
}

std::string Point::toString() const
{
    return (boost::format("%f,%f") % x % y).str();
}

bool Point::operator==(const Point &other) const
{
    return (x == other.x && y == other.y);
}

bool Point::operator!=(const Point &other) const
{
    return (x != other.x || y != other.y);
}

Point Point::operator+(const Point &other)
{
    Point res(x + other.x, y + other.y);
    return res;
}

Point Point::operator-(const Point &other)
{
    Point res(x - other.x, y - other.y);
    return res;
}

Point &Point::operator+=(const Point &other)
{
    x += other.x;
    y += other.y;
    return *this;
}

Point &Point::operator-=(const Point &other)
{
    x -= other.x;
    y -= other.y;
    return *this;
}

void Point::update(Point another)
{
    x = another.x;
    y = another.y;
}

Point Point::semiPoint(Point target, double pos) const
{
    if (pos < 0) return *this;
    if (pos >= 1.0) return target;

    double nposx = target.x - x;
    double nposy = target.y - y;

    nposx *= pos;
    nposy *= pos;

    nposx += x;
    nposy += y;

    return Point(nposx, nposy);
}

Point Point::traverse(Point target, double now, double time) const
{
    // Travel to another point, in specific time, specify now as current time
    if (now <= 0) return *this;
    if (now > time) return target;

    double nposx, nposy;
    nposx = target.x - x;
    nposy = target.y - y;
    nposx = (nposx * now / time) + x;
    nposy = (nposy * now / time) + y;

    return Point(nposx, nposy);
}

double Point::length(Point another)
{
    double dx = x - another.x;
    double dy = y - another.y;
    if (dx < 0) dx *= -1;
    if (dy < 0) dy *= -1;

    // return sqrt(dx*dx + dy*dy);
    return dx+dy;
}

double Point::lengthGeo(Point another)
{
    double dlat = (x - another.x) * blieng::PI / 180.0;
    double dlon = (y - another.y) * blieng::PI / 180.0;

    double a = sin(dlat / 2) * sin(dlat / 2) +
        cos(another.x * blieng::PI / 180.0) * cos(x * blieng::PI / 180.0) *
        sin(dlon / 2) * sin(dlon / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));
    double d = blieng::RADIUS * c;

    // Meters
    return d;
}

Point Point::geoToMeters()
{
    double latF = 2 * blieng::RADIUS * blieng::PI / 360.0;
    double lngF = latF * cos(x * blieng::PI / 180.0);

    return Point(x * latF, y * lngF);
}

blieng::Complex Point::toComplex() const
{
    return Complex(x, y);
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/arch/common/detail/scalar/f_invtrig.hpp>
#include <boost/simd/arch/common/detail/scalar/d_invtrig.hpp>
#include <boost/simd/arch/common/detail/simd/f_invtrig.hpp>
#include <boost/simd/arch/common/detail/simd/d_invtrig.hpp>

#endif


/*
 Copyright (C) 2016 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 <ored/portfolio/builders/cms.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;

namespace ore {
namespace data {

namespace {
Real parseCcyReversion(const std::map<string, string>& p, const std::string& ccy) {
    // either we have a ccy specific reversion or require a ccy independent reversion parameter
    if (p.find("MeanReversion_" + ccy) != p.end())
        return parseReal(p.at("MeanReversion_" + ccy));
    else
        return parseReal(p.at("MeanReversion"));
}
} // namespace

GFunctionFactory::YieldCurveModel ycmFromString(const string& s) {
    if (s == "Standard")
        return GFunctionFactory::Standard;
    else if (s == "ExactYield")
        return GFunctionFactory::ExactYield;
    else if (s == "ParallelShifts")
        return GFunctionFactory::ParallelShifts;
    else if (s == "NonParallelShifts")
        return GFunctionFactory::NonParallelShifts;
    else
        QL_FAIL("unknown string for YieldCurveModel");
}

boost::shared_ptr<FloatingRateCouponPricer> AnalyticHaganCmsCouponPricerBuilder::engineImpl(const Currency& ccy) {

    const string& ccyCode = ccy.code();
    Real rev = parseCcyReversion(engineParameters_, ccyCode);
    string ycmstr = engineParameter("YieldCurveModel");
    GFunctionFactory::YieldCurveModel ycm = ycmFromString(ycmstr);

    Handle<Quote> revQuote(boost::shared_ptr<Quote>(new SimpleQuote(rev)));
    Handle<SwaptionVolatilityStructure> vol = market_->swaptionVol(ccyCode, configuration(MarketContext::pricing));

    boost::shared_ptr<FloatingRateCouponPricer> pricer = boost::make_shared<AnalyticHaganPricer>(vol, ycm, revQuote);

    // Return the cached pricer
    return pricer;
}

boost::shared_ptr<FloatingRateCouponPricer> NumericalHaganCmsCouponPricerBuilder::engineImpl(const Currency& ccy) {

    const string& ccyCode = ccy.code();
    Real rev = parseCcyReversion(engineParameters_, ccyCode);
    string ycmstr = engineParameter("YieldCurveModel");
    GFunctionFactory::YieldCurveModel ycm = ycmFromString(ycmstr);
    Rate llim = parseReal(engineParameter("LowerLimit"));
    Rate ulim = parseReal(engineParameter("UpperLimit"));
    Real prec = parseReal(engineParameter("Precision"));

    Handle<Quote> revQuote(boost::shared_ptr<Quote>(new SimpleQuote(rev)));
    Handle<SwaptionVolatilityStructure> vol = market_->swaptionVol(ccyCode, configuration(MarketContext::pricing));

    boost::shared_ptr<FloatingRateCouponPricer> pricer =
        boost::make_shared<NumericHaganPricer>(vol, ycm, revQuote, llim, ulim, prec);

    // Return the cached pricer
    return pricer;
}

boost::shared_ptr<FloatingRateCouponPricer> LinearTSRCmsCouponPricerBuilder::engineImpl(const Currency& ccy) {

    const string& ccyCode = ccy.code();
    Real rev = parseCcyReversion(engineParameters_, ccyCode);
    string policy = engineParameter("Policy");

    Handle<Quote> revQuote(boost::shared_ptr<Quote>(new SimpleQuote(rev)));
    Handle<SwaptionVolatilityStructure> vol = market_->swaptionVol(ccyCode, configuration(MarketContext::pricing));
    Handle<YieldTermStructure> yts = market_->discountCurve(ccyCode, configuration(MarketContext::pricing));

    string lowerBoundStr =
        (vol->volatilityType() == ShiftedLognormal) ? "LowerRateBoundLogNormal" : "LowerRateBoundNormal";
    string upperBoundStr =
        (vol->volatilityType() == ShiftedLognormal) ? "UpperRateBoundLogNormal" : "UpperRateBoundNormal";

    LinearTsrPricer::Settings settings;
    if (policy == "RateBound") {
        Real lower = parseReal(engineParameter(lowerBoundStr));
        Real upper = parseReal(engineParameter(upperBoundStr));
        settings.withRateBound(lower, upper);
    } else if (policy == "VegaRatio") {
        Real lower = parseReal(engineParameter(lowerBoundStr));
        Real upper = parseReal(engineParameter(upperBoundStr));
        Real vega = parseReal(engineParameter("VegaRatio"));
        settings.withVegaRatio(vega, lower, upper);
    } else if (policy == "PriceThreshold") {
        Real lower = parseReal(engineParameter(lowerBoundStr));
        Real upper = parseReal(engineParameter(upperBoundStr));
        Real threshold = parseReal(engineParameter("PriceThreshold"));
        settings.withPriceThreshold(threshold, lower, upper);
    } else if (policy == "BsStdDev") {
        Real lower = parseReal(engineParameter(lowerBoundStr));
        Real upper = parseReal(engineParameter(upperBoundStr));
        Real stddevs = parseReal(engineParameter("BSStdDevs"));
        settings.withPriceThreshold(stddevs, lower, upper);
    } else
        QL_FAIL("unknown string for policy parameter");

    boost::shared_ptr<FloatingRateCouponPricer> pricer =
        boost::make_shared<LinearTsrPricer>(vol, revQuote, yts, settings);

    // Return the cached pricer
    return pricer;
}
} // namespace data
} // namespace ore


#include <cstddef>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include <boost/program_options.hpp>
#include <insertionfinder/algorithm.hpp>
#include <insertionfinder/case.hpp>
#include <insertionfinder/cube.hpp>
#include "../utils/encoding.hpp"
#include "commands.hpp"
using std::size_t;
namespace po = boost::program_options;
using InsertionFinder::Algorithm;
using InsertionFinder::AlgorithmError;
using InsertionFinder::Case;
using InsertionFinder::Cube;
namespace CLI = InsertionFinder::CLI;
namespace Details = InsertionFinder::Details;

void CLI::generate_algorithms(const po::variables_map& vm) {
    const std::vector<std::string> filenames =
        vm.count("file") ? vm["file"].as<std::vector<std::string>>() : std::vector<std::string>();
    const std::vector<std::string> algfilenames =
        vm.count("algfile") ? vm["algfile"].as<std::vector<std::string>>() : std::vector<std::string>();

    std::unordered_map<Cube, Case> map;

    for (const std::string& name: filenames) {
        std::ifstream fin(name);
        if (fin.fail()) {
            std::cerr << "Failed to open file " << name << std::endl;
            continue;
        }
        while (!fin.eof()) {
            std::string line;
            std::getline(fin, line);
            Algorithm algorithm;
            try {
                algorithm = Algorithm(line);
            } catch (const AlgorithmError& e) {
                std::cerr << "Invalid algorithm: " << e.what() << std::endl;
                continue;
            }
            algorithm.simplify();
            algorithm.normalize();
            algorithm.detect_rotation();
            Cube cube = Cube() * algorithm;
            if (cube.mask() == 0) {
                continue;
            }

            if (auto node = map.find(cube); node != map.end() && node->second.contains_algorithm(algorithm)) {
                continue;
            }
            std::vector<Algorithm> algorithms = vm.count("symmetrics-only")
                ? algorithm.generate_symmetrics()
                : algorithm.generate_similars();
            for (Algorithm& alg: algorithms) {
                Cube cube = Cube() * alg;
                if (auto node = map.find(cube); node != map.end()) {
                    node->second.add_algorithm(std::move(alg));
                } else {
                    Case _case(cube);
                    _case.add_algorithm(std::move(alg));
                    map.emplace(cube, std::move(_case));
                }
            }
        }
    }

    size_t size = map.size();
    std::vector<Case> cases;
    cases.reserve(size);
    for (auto& node: map) {
        cases.emplace_back(std::move(node.second));
    }
    std::sort(cases.begin(), cases.end(), [](const Case& x, const Case& y) {return Case::compare(x, y) < 0;});

    std::shared_ptr<std::ostream> out;
    if (algfilenames.empty()) {
        out.reset(&std::cout, [](std::ostream*) {});
    } else {
        const std::string& name = algfilenames.front();
        out = std::make_shared<std::ofstream>(name, std::ios::out | std::ios::binary);
        if (out->fail()) {
            throw CLI::CommandExecutionError("Failed to open output file " + name);
        }
    }
    Details::write_varuint(*out, size);
    for (Case& _case: cases) {
        _case.sort_algorithms();
        _case.save_to(*out);
    }
}


#include <boost/test/unit_test.hpp>
#include "algorithms/math/hamming_distance.hpp"

BOOST_AUTO_TEST_SUITE(TestHammingDistance)

BOOST_AUTO_TEST_CASE(str_empty_strings)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(0 == solution.HammingDistanceForStrings(std::string(),
                                                        std::string()));
}

BOOST_AUTO_TEST_CASE(str_one_empty_string)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(-1 == solution.HammingDistanceForStrings(std::string("123"),
                                                        std::string()));

    BOOST_CHECK(-1 == solution.HammingDistanceForStrings(std::string(),
                                                        std::string("1234")));
}

BOOST_AUTO_TEST_CASE(str_strings_of_dif_size)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(-1 == solution.HammingDistanceForStrings("asdf", "bv"));
    BOOST_CHECK(-1 == solution.HammingDistanceForStrings("a", "bvt"));
}

BOOST_AUTO_TEST_CASE(str_equal_strings)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(0 == solution.HammingDistanceForStrings("abc1", "abc1"));
}

BOOST_AUTO_TEST_CASE(str_dif_strings)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(2 == solution.HammingDistanceForStrings("abb2", "abc1"));
    BOOST_CHECK(4 == solution.HammingDistanceForStrings("zabbee", "aabc1d"));
}

BOOST_AUTO_TEST_CASE(int_nulls)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(0 == solution.HammingDistanceForInts(0, 0));
    BOOST_CHECK(0 == solution.HammingDistanceForIntsFast(0, 0));
}

BOOST_AUTO_TEST_CASE(int_diff_values)
{
    HammingDistance::Solution solution;
    BOOST_CHECK(1 == solution.HammingDistanceForInts(1, 0));
    BOOST_CHECK(1 == solution.HammingDistanceForInts(2, 0));
    BOOST_CHECK(8 == solution.HammingDistanceForInts(143, 5628));
    BOOST_CHECK(25 == solution.HammingDistanceForInts(-143, 5628));

    BOOST_CHECK(1 == solution.HammingDistanceForIntsFast(1, 0));
    BOOST_CHECK(1 == solution.HammingDistanceForIntsFast(2, 0));
    BOOST_CHECK(8 == solution.HammingDistanceForIntsFast(143, 5628));
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2009 by Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_INDEX_HPP
#define BOOST_NUMERIC_BINDINGS_INDEX_HPP

#include <boost/mpl/if.hpp>
#include <boost/mpl/max.hpp>
#include <boost/numeric/bindings/rank.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>

namespace boost {
namespace numeric {
namespace bindings {

template< typename T >
struct addressing_index_minor {
    typedef typename mpl::if_<
        is_column_major< T >,
        tag::addressing_index<1>,
        tag::addressing_index<
            mpl::max< tag::matrix, rank< T > >::type::value
        >
    >::type type;
};

template< typename T >
struct addressing_index_major {
    typedef typename mpl::if_<
        is_column_major< T >,
        tag::addressing_index<
            mpl::max< tag::matrix, rank< T > >::type::value
        >,
        tag::addressing_index<1>
    >::type type;
};

template< typename AddressingIndex, typename TransTag >
struct addressing_index_trans {
    typedef AddressingIndex type;
};

template<>
struct addressing_index_trans< tag::addressing_index<1>, tag::transpose > {
    typedef tag::addressing_index<2> type;
};

template<>
struct addressing_index_trans< tag::addressing_index<1>, tag::conjugate > {
    typedef tag::addressing_index<2> type;
};

template<>
struct addressing_index_trans< tag::addressing_index<2>, tag::transpose > {
    typedef tag::addressing_index<1> type;
};

template<>
struct addressing_index_trans< tag::addressing_index<2>, tag::conjugate > {
    typedef tag::addressing_index<1> type;
};

} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_MULTIBODY_COST_BASE_HPP_
#define CROCODDYL_MULTIBODY_COST_BASE_HPP_

#include "crocoddyl/multibody/states/multibody.hpp"
#include "crocoddyl/core/activation-base.hpp"
#include <pinocchio/multibody/data.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

namespace crocoddyl {

struct CostDataAbstract;  // forward declaration

class CostModelAbstract {
 public:
  CostModelAbstract(StateMultibody& state, ActivationModelAbstract& activation, unsigned int const& nu,
                    const bool& with_residuals = true);
  CostModelAbstract(StateMultibody& state, ActivationModelAbstract& activation, const bool& with_residuals = true);
  CostModelAbstract(StateMultibody& state, unsigned int const& nr, unsigned int const& nu,
                    const bool& with_residuals = true);
  CostModelAbstract(StateMultibody& state, unsigned int const& nr, const bool& with_residuals = true);
  ~CostModelAbstract();

  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
                    const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
                        const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true) = 0;
  virtual boost::shared_ptr<CostDataAbstract> createData(pinocchio::Data* const data);

  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);

  StateMultibody& get_state() const;
  ActivationModelAbstract& get_activation() const;
  unsigned int const& get_nu() const;

 protected:
  StateMultibody& state_;
  ActivationModelAbstract& activation_;
  unsigned int nu_;
  bool with_residuals_;
  Eigen::VectorXd unone_;

#ifdef PYTHON_BINDINGS

 public:
  void calc_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
                 const Eigen::VectorXd& u = Eigen::VectorXd()) {
    if (u.size() == 0) {
      calc(data, x);
    } else {
      calc(data, x, u);
    }
  }

  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u, const bool& recalc) {
    calcDiff(data, x, u, recalc);
  }
  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u) {
    calcDiff(data, x, u, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x) {
    calcDiff(data, x, unone_, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x, const bool& recalc) {
    calcDiff(data, x, unone_, recalc);
  }

#endif
};

struct CostDataAbstract {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  template <typename Model>
  CostDataAbstract(Model* const model, pinocchio::Data* const data)
      : pinocchio(data), activation(model->get_activation().createData()), cost(0.) {
    const int& ndx = model->get_state().get_ndx();
    const int& nu = model->get_nu();
    const int& nr = model->get_activation().get_nr();
    Lx = Eigen::VectorXd::Zero(ndx);
    Lu = Eigen::VectorXd::Zero(nu);
    Lxx = Eigen::MatrixXd::Zero(ndx, ndx);
    Lxu = Eigen::MatrixXd::Zero(ndx, nu);
    Luu = Eigen::MatrixXd::Zero(nu, nu);
    r = Eigen::VectorXd::Zero(nr);
    Rx = Eigen::MatrixXd::Zero(nr, ndx);
    Ru = Eigen::MatrixXd::Zero(nr, nu);
  }

  pinocchio::Data* pinocchio;
  boost::shared_ptr<ActivationDataAbstract> activation;
  double cost;
  Eigen::VectorXd Lx;
  Eigen::VectorXd Lu;
  Eigen::MatrixXd Lxx;
  Eigen::MatrixXd Lxu;
  Eigen::MatrixXd Luu;
  Eigen::VectorXd r;
  Eigen::MatrixXd Rx;
  Eigen::MatrixXd Ru;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_MULTIBODY_COST_BASE_HPP_


// (C) Copyright David Abrahams and Jeremy Siek 2000-2001. Permission to copy,
// use, modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided "as is"
// without express or implied warranty, and with no claim as to its suitability
// for any purpose.
//
// Revision History:
// 04 Jan 2001  Factored counting_iterator stuff into
//              boost/counting_iterator.hpp (David Abrahams)

#ifndef BOOST_INTEGER_RANGE_HPP_
#define BOOST_INTEGER_RANGE_HPP_

#include <boost/config.hpp>
#include <boost/counting_iterator.hpp>

namespace boost {

//=============================================================================
// Counting Iterator and Integer Range Class

template <class IntegerType>
struct integer_range {
    typedef typename counting_iterator_generator<IntegerType>::type iterator;

    typedef iterator const_iterator;
    typedef IntegerType value_type;
    typedef std::ptrdiff_t difference_type;
    typedef IntegerType reference;
    typedef IntegerType const_reference;
    typedef const IntegerType* pointer;
    typedef const IntegerType* const_pointer;
    typedef IntegerType size_type;

    integer_range(IntegerType start, IntegerType finish)
        : m_start(start), m_finish(finish) { }

    iterator begin() const { return iterator(m_start); }
    iterator end() const { return iterator(m_finish); }
    size_type size() const { return m_finish - m_start; }
    bool empty() const { return m_finish == m_start; }
    void swap(integer_range& x) {
        std::swap(m_start, x.m_start);
        std::swap(m_finish, x.m_finish);
    }
protected:
    IntegerType m_start, m_finish;
};

template <class IntegerType>
inline integer_range<IntegerType>
make_integer_range(IntegerType first, IntegerType last)
{
  return integer_range<IntegerType>(first, last);
}

} // namespace boost

#endif // BOOST_INTEGER_RANGE_HPP_


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/array.hpp>

#include <boost/hana/detail/assert.hpp>

#include <array>
using namespace boost::hana;

template <int ...i>
 auto array = std::array<int, sizeof...(i)>{{i...}};

int main() {
    BOOST_HANA_CONSTANT_ASSERT(is_empty(array<>));
    BOOST_HANA_CONSTANT_ASSERT(!is_empty(array<0>));
    BOOST_HANA_CONSTANT_ASSERT(!is_empty(array<0, 1>));
}


/*=============================================================================
    Copyright (c) 2015 Agustin K-ballo Berge
    Copyright (c) 2015 Kohei Takahashi

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/

#ifndef BOOST_FUSION_SUPPORT_DETAIL_INDEX_SEQUENCE_06232015_1038
#define BOOST_FUSION_SUPPORT_DETAIL_INDEX_SEQUENCE_06232015_1038

#include <boost/fusion/support/config.hpp>
#include <cstddef>

namespace boost { namespace fusion { namespace detail
{
    template <std::size_t ...Ints>
    struct index_sequence
    {
        typedef std::size_t value_type;

        BOOST_CONSTEXPR BOOST_FUSION_GPU_ENABLED
        static std::size_t size() BOOST_NOEXCEPT
        { return sizeof...(Ints); }

        // non standard extension
        typedef index_sequence type;
    };

    template <typename Left, typename Right>
    struct _make_index_sequence_join;

    template <std::size_t... Left, std::size_t... Right>
    struct _make_index_sequence_join<
        index_sequence<Left...>, index_sequence<Right...>
    > : index_sequence<Left..., (sizeof...(Left) + Right)...>
    {};

    template <std::size_t N>
    struct make_index_sequence
      : _make_index_sequence_join<
            typename make_index_sequence<N / 2>::type
          , typename make_index_sequence<N - N / 2>::type
        >
    {};

    template <>
    struct make_index_sequence<1>
      : index_sequence<0>
    {};

    template <>
    struct make_index_sequence<0>
      : index_sequence<>
    {};
}}}

#endif


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

struct op {
    string instruction;
    int value;
};

bool run(const vector<struct op> & program, int *var) {
    assert(var);
    int pc = 0;

    map<int, bool> visited;

    while (pc >= 0 && pc < program.size()) {
        if (visited[pc]) {
            return false;
        }

        visited[pc] = true;
        const auto & op = program[pc];

        if (op.instruction == "acc") {
            *var += op.value;
        } else if (op.instruction == "jmp") {
            pc += (op.value - 1);
        } else if (op.instruction == "nop") {
            // nothing
        } else {
            assert(false);
        }

        pc++;
    }

    return true;
}

int main() {
    ifstream file ("2020/8.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    int answer1 = 0;
    int answer2 = 0;

    vector<struct op> program;

    string line;
    while (getline(file, line, '\n')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }

        smatch matches;
        regex e1 ("([a-z]+) ([-+]\\d+)");
        assert(regex_search(line, matches, e1));

        struct op op;
        op.instruction = matches[1];
        op.value = stoi(matches[2]);

        program.push_back(op);
    }
    file.close();

    // Part 1
    run(program, &answer1);

    // Part 2
    for (int i = 0; i < program.size(); i++) {
        auto & op = program[i];

        if (op.instruction == "nop" ) {
            op.instruction = "jmp";
        } else if (op.instruction == "jmp" ) {
            op.instruction = "nop";
        }

        answer2 = 0;
        if (run(program, &answer2)) {
            break;
        };

        // Flip back
        if (op.instruction == "nop" ) {
            op.instruction = "jmp";
        } else if (op.instruction == "jmp" ) {
            op.instruction = "nop";
        }
    }

    cout << "Answer 8.1: " << answer1 << endl;
    cout << "Answer 8.2: " << answer2 << endl;
}


/*
 * PropertyHandler.hpp
 *
 *  Created on: Oct 27, 2014
 *      Author: Bloeschm
 */

#ifndef LWF_PropertyHandler_HPP_
#define LWF_PropertyHandler_HPP_

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include "lightweight_filtering/common.hpp"
#include <unordered_map>
#include <unordered_set>

namespace LWF{

template<typename TYPE>
class Register{
 public:
  typedef boost::property_tree::ptree ptree;
  Register(){};
  virtual ~Register(){};
  std::map<TYPE*,std::string> registerMap_;
  std::unordered_set<TYPE*> zeros_;
  void registerZero(TYPE& var){
    if(zeros_.count(&var)!=0) std::cout << "Property Handler Error: Zero variable already registered." << std::endl;
    zeros_.insert(&var);
  }
  void registerScalar(std::string str, TYPE& var){
    if(registerMap_.count(&var)!=0) std::cout << "Property Handler Error: Variable already registered to " << str << "." << std::endl;
    registerMap_[&var] = str;
  }
  void registerVector(std::string str, Eigen::Matrix<TYPE,3,1>& var){
    registerScalar(str + "_x",var(0));
    registerScalar(str + "_y",var(1));
    registerScalar(str + "_z",var(2));
  }
  void registerQuaternion(std::string str, Eigen::Quaternion<TYPE>& var){
    registerScalar(str + "_w",var.w());
    registerScalar(str + "_x",var.x());
    registerScalar(str + "_y",var.y());
    registerScalar(str + "_z",var.z());
  }
  template<int N, int M>
  void registerMatrix(std::string str, Eigen::Matrix<TYPE,N,M>& var){
    for(unsigned int i=0;i<N;i++){
      for(unsigned int j=0;j<M;j++){
        registerScalar(str + "_" + std::to_string(i) + "_" + std::to_string(j),var(i,j));
      }
    }
  }
  template <typename Derived>
  void registerDiagonalMatrix(std::string str, Eigen::MatrixBase<Derived>& var){
    const int N = var.rows();
    for(unsigned int i=0;i<N;i++){
      registerScalar(str + "_" + std::to_string(i),var(i,i));
    }
    for(unsigned int i=0;i<N;i++){
      for(unsigned int j=0;j<N;j++){
        if(i!=j) registerZero(var(i,j));
      }
    }
  }
  template<int N>
  void registerScaledUnitMatrix(std::string str, Eigen::Matrix<TYPE,N,N>& var){
    for(unsigned int i=0;i<N;i++){
      registerScalar(str,var(i,i));
    }
    for(unsigned int i=0;i<N;i++){
      for(unsigned int j=0;j<N;j++){
        if(i!=j) registerZero(var(i,j));
      }
    }
  }
  void removeScalarByStr(std::string str){ // slow
    bool found = false;
    for(auto it=registerMap_.begin(); it != registerMap_.end(); ++it){
      if(it->second == str){
        registerMap_.erase(it);
        found = true;
        break;
      }
    }
    if(!found) std::cout << "Property Handler Error: Cannot remove variable with str = " << str << std::endl;
  }
  void removeScalarByVar(TYPE& var){
    if(registerMap_.count(&var)==0) std::cout << "Property Handler Error: Cannot remove variable, does not exist!" << std::endl;
    registerMap_.erase(&var);
  }
  void buildPropertyTree(ptree& pt){
    for(typename std::map<TYPE*,std::string>::iterator it=registerMap_.begin(); it != registerMap_.end(); ++it){
      pt.put(it->second, *(it->first));
    }
  }
  void readPropertyTree(ptree& pt){
    for(typename std::map<TYPE*,std::string>::iterator it=registerMap_.begin(); it != registerMap_.end(); ++it){
      *(it->first) = pt.get<TYPE>(it->second);
    }
    for(typename std::unordered_set<TYPE*>::iterator it=zeros_.begin(); it != zeros_.end(); ++it){
      **it = static_cast<TYPE>(0);
    }
  }
};

class PropertyHandler{
 public:
  typedef boost::property_tree::ptree ptree;
  PropertyHandler(){};
  virtual ~PropertyHandler(){};
  Register<bool> boolRegister_;
  Register<int> intRegister_;
  Register<double> doubleRegister_;
  Register<std::string> stringRegister_;
  std::unordered_map<std::string,PropertyHandler*> subHandlers_;
  void buildPropertyTree(ptree& pt){
    boolRegister_.buildPropertyTree(pt);
    intRegister_.buildPropertyTree(pt);
    doubleRegister_.buildPropertyTree(pt);
    stringRegister_.buildPropertyTree(pt);
    for(typename std::unordered_map<std::string,PropertyHandler*>::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){
      ptree ptsub;
      it->second->buildPropertyTree(ptsub);
      pt.add_child(it->first,ptsub);
    }
  }
  void readPropertyTree(ptree& pt){
    boolRegister_.readPropertyTree(pt);
    intRegister_.readPropertyTree(pt);
    doubleRegister_.readPropertyTree(pt);
    stringRegister_.readPropertyTree(pt);
    for(typename std::unordered_map<std::string,PropertyHandler*>::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){
      ptree ptsub;
      ptsub = pt.get_child(it->first);
      it->second->readPropertyTree(ptsub);
    }
  }
  void registerSubHandler(std::string str,PropertyHandler& subHandler){
    if(subHandlers_.count(str)!=0) std::cout << "Property Handler Error: subHandler with name " << str << " already exists" << std::endl;
    subHandlers_[str] = &subHandler;
  }
  void writeToInfo(const std::string &filename){
    ptree pt;
    buildPropertyTree(pt);
    write_info(filename,pt);
  }
  void readFromInfo(const std::string &filename){
    ptree ptDefault;
    buildPropertyTree(ptDefault);
    ptree pt;
    try{
      read_info(filename,pt);
      readPropertyTree(pt);
      refreshProperties();
      for(typename std::unordered_map<std::string,PropertyHandler*>::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){
        it->second->refreshProperties();
      }
    } catch (boost::property_tree::ptree_error& e){
      std::cout << "An exception occurred. " << e.what() << std::endl;
      std::string newFilename = filename + "_new";
      std::cout << "\033[31mWriting a new info-file to " << newFilename << "\033[0m" << std::endl;
      write_info(newFilename,ptDefault);

      throw std::runtime_error("fail");

      refreshProperties();
      for(typename std::unordered_map<std::string,PropertyHandler*>::iterator it=subHandlers_.begin(); it != subHandlers_.end(); ++it){
        it->second->refreshProperties();
      }
    }
  }
  virtual void refreshProperties(){};
};

}

#endif /* LWF_PropertyHandler_HPP_ */


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

#include <gtest/gtest.h>
#include <fuse_core/eigen_gtest.h>

#include <ceres/autodiff_cost_function.h>
#include <ceres/gradient_checker.h>
#include <Eigen/Dense>

#include <limits>
#include <vector>

TEST(CostFunction, evaluateCostFunction)
{
  // Create cost function
  const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 };
  const fuse_core::Matrix8d covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal();

  const double dt{ 0.1 };
  const fuse_core::Matrix8d sqrt_information{ covariance.inverse().llt().matrixU() };

  const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information };

  // Evaluate cost function
  const double position1[] = {0.0, 0.0};
  const double yaw1[] = {0.0};
  const double vel_linear1[] = {1.0, 0.0};
  const double vel_yaw1[] = {1.570796327};
  const double acc_linear1[] = {1.0, 0.0};

  const double position2[] = {0.105, 0.0};
  const double yaw2[] = {0.1570796327};
  const double vel_linear2[] = {1.1, 0.0};
  const double vel_yaw2[] = {1.570796327};
  const double acc_linear2[] = {1.0, 0.0};

  const double* parameters[] =
  {
    position1, yaw1, vel_linear1, vel_yaw1, acc_linear1,
    position2, yaw2, vel_linear2, vel_yaw2, acc_linear2
  };

  fuse_core::Vector8d residuals;

  const auto& block_sizes = cost_function.parameter_block_sizes();
  const auto num_parameter_blocks = block_sizes.size();

  const auto num_residuals = cost_function.num_residuals();

  std::vector<fuse_core::MatrixXd> J(num_parameter_blocks);
  std::vector<double*> jacobians(num_parameter_blocks);

  for (size_t i = 0; i < num_parameter_blocks; ++i)
  {
    J[i].resize(num_residuals, block_sizes[i]);
    jacobians[i] = J[i].data();
  }

  EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data()));

  // We cannot use std::numeric_limits<double>::epsilon() tolerance because with the expected state2 above the residuals
  // are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16
  EXPECT_MATRIX_NEAR(fuse_core::Vector8d::Zero(), residuals, 1e-15);

  // Check jacobians are correct using a gradient checker
  ceres::NumericDiffOptions numeric_diff_options;
  ceres::GradientChecker gradient_checker(&cost_function, NULL, numeric_diff_options);

  // We cannot use std::numeric_limits<double>::epsilon() tolerance because the worst relative error is 5.26356e-10
  ceres::GradientChecker::ProbeResults probe_results;
  // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, but Probe actually
  // returns true and the jacobians are correct according to the gradient checker numeric differentiation
  // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << probe_results.error_log;

  // Create cost function using automatic differentiation on the cost functor
  ceres::AutoDiffCostFunction<fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2>
      cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information));

  // Evaluate cost function that uses automatic differentiation
  std::vector<fuse_core::MatrixXd> J_autodiff(num_parameter_blocks);
  std::vector<double*> jacobians_autodiff(num_parameter_blocks);

  for (size_t i = 0; i < num_parameter_blocks; ++i)
  {
    J_autodiff[i].resize(num_residuals, block_sizes[i]);
    jacobians_autodiff[i] = J_autodiff[i].data();
  }

  EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data()));

  const Eigen::IOFormat HeavyFmt(
      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");

  for (size_t i = 0; i < num_parameter_blocks; ++i)
  {
    EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits<double>::epsilon())
      << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt)
      << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt);
  }
}

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


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_BLOCK_BASIC_SHACAL_HPP
#define CRYPTO3_BLOCK_BASIC_SHACAL_HPP

#include <boost/crypto3/block/detail/shacal/shacal_policy.hpp>
#include <boost/crypto3/block/detail/shacal/shacal1_policy.hpp>

#include <boost/static_assert.hpp>

#ifdef CRYPTO3_BLOCK_SHOW_PROGRESS
#include <cstdio>
#endif

namespace boost {
    namespace crypto3 {
        namespace block {
            /*!
             * @ingroup block
             *
             * Encrypt implemented directly from the SHA standard as found at
             * http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
             *
             * Decrypt is a straight-forward inverse
             *
             * In SHA terminology:
             * - plaintext = H^(i-1)
             * - ciphertext = H^(i)
             * - key = M^(i)
             * - schedule = W
             *
             * The algorithms for SHA(-0) and SHA-1 are identical apart from the
             * key scheduling, so encapsulate that as a class that takes an
             * already-prepared schedule.  (Constructor is protected to help keep
             * people from accidentally giving it just a key in a schedule.)
             */
            class basic_shacal {
            protected:
                typedef detail::shacal_policy policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef policy_type::word_type word_type;

                constexpr static const std::size_t key_bits = policy_type::key_bits;
                constexpr static const std::size_t key_words = policy_type::key_words;
                typedef policy_type::key_type key_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef policy_type::block_type block_type;

                constexpr static const std::size_t rounds = policy_type::rounds;
                typedef policy_type::schedule_type schedule_type;

            protected:
                basic_shacal(const schedule_type &s) : schedule(s) {
                }

                virtual ~basic_shacal() {
                    schedule.fill(0);
                }

            public:
                inline block_type encrypt(const block_type &plaintext) const {
                    return encrypt_block(schedule, plaintext);
                }

                inline block_type decrypt(const block_type &ciphertext) const {
                    return decrypt_block(schedule, ciphertext);
                }

            private:
                schedule_type schedule;

                inline static block_type encrypt_block(const schedule_type &schedule, const block_type &plaintext) {

#ifdef CRYPTO3_BLOCK_SHOW_PROGRESS
                    for (unsigned t = 0; t < block_words; ++t) {
                        std::printf(word_bits == 32 ? "H[%d] = %.8x\n" : "H[%d] = %.16lx\n", t, plaintext[t]);
                    }
#endif

                    // Initialize working variables with block
                    word_type a = plaintext[0], b = plaintext[1], c = plaintext[2], d = plaintext[3], e = plaintext[4];

                    // Encipher block
#ifdef CRYPTO3_BLOCK_NO_OPTIMIZATION

                    for (unsigned t = 0; t < rounds; ++t) {
                        word_type T = policy_type::rotl<5>(a) + policy_type::f(t, b, c, d) + e +
                                      policy_type::constants[t] + round_constants_words[t];

                        e = d;
                        d = c;
                        c = policy_type::rotl<30>(b);
                        b = a;
                        a = T;

#ifdef CRYPTO3_BLOCK_SHOW_PROGRESS
                        printf(word_bits == 32 ? "t = %2d: %.8x %.8x %.8x %.8x %.8x\n" :
                                                 "t = %2d: %.16lx %.16lx %.16lx %.16lx %.16lx\n",
                               t, a, b, c, d, e);
#endif
                    }

#else    // CRYPTO3_BLOCK_NO_OPTIMIZATION

#ifdef CRYPTO3_BLOCK_SHOW_PROGRESS
#define CRYPTO3_BLOCK_SHACAL1_TRANSFORM_PROGRESS                                                                      \
    printf(word_bits == 32 ? "t = %2d: %.8x %.8x %.8x %.8x %.8x\n" : "t = %2d: %.16lx %.16lx %.16lx %.16lx %.16lx\n", \
           t, a, b, c, d, e);
#else
#define CRYPTO3_BLOCK_SHACAL1_TRANSFORM_PROGRESS
#endif

#define CRYPTO3_BLOCK_SHACAL1_TRANSFORM                                                                               \
    word_type T = policy_type::rotl<5>(a) + policy_type::f(t, b, c, d) + e + policy_type::constants[t] + schedule[t]; \
    e = d;                                                                                \
    d = c;                                                                                \
    c = policy_type::rotl<30>(b);                                                                                \
    b = a;                                                                                \
    a = T;                                                                                \
    CRYPTO3_BLOCK_SHACAL1_TRANSFORM_PROGRESS

                    BOOST_STATIC_ASSERT(rounds == 80);
                    BOOST_STATIC_ASSERT(rounds % block_words == 0);
                    for (unsigned t = 0; t < 20;) {
                        for (int n = block_words; n--; ++t) {
                            CRYPTO3_BLOCK_SHACAL1_TRANSFORM
                        }
                    }
                    for (unsigned t = 20; t < 40;) {
                        for (int n = block_words; n--; ++t) {
                            CRYPTO3_BLOCK_SHACAL1_TRANSFORM
                        }
                    }
                    for (unsigned t = 40; t < 60;) {
                        for (int n = block_words; n--; ++t) {
                            CRYPTO3_BLOCK_SHACAL1_TRANSFORM
                        }
                    }
                    for (unsigned t = 60; t < 80;) {
                        for (int n = block_words; n--; ++t) {
                            CRYPTO3_BLOCK_SHACAL1_TRANSFORM
                        }
                    }

#endif

                    return {{a, b, c, d, e}};
                }

                inline static block_type decrypt_block(const schedule_type &schedule, const block_type &ciphertext) {

#ifdef CRYPTO3_BLOCK_SHOW_PROGRESS
                    for (unsigned t = 0; t < block_words; ++t) {
                        std::printf(word_bits == 32 ? "H[%d] = %.8x\n" : "H[%d] = %.16lx\n", t, ciphertext[t]);
                    }
#endif

                    // Initialize working variables with block
                    word_type a = ciphertext[0], b = ciphertext[1], c = ciphertext[2], d = ciphertext[3],
                              e = ciphertext[4];

                    // Decipher block
                    for (unsigned t = rounds; t--;) {
                        word_type T = a;

                        a = b;
                        b = policy_type::rotr<30>(c);
                        c = d;
                        d = e;
                        e = T - policy_type::rotl<5>(a) - policy_type::f(t, b, c, d) - policy_type::constants[t] -
                            schedule[t];

#ifdef CRYPTO3_BLOCK_SHOW_PROGRESS
                        std::printf(word_bits == 32 ? "t = %2d: %.8x %.8x %.8x %.8x %.8x\n" :
                                                      "t = %2d: %.16lx %.16lx %.16lx %.16lx %.16lx\n",
                                    t, a, b, c, d, e);
#endif
                    }

                    return {{a, b, c, d, e}};
                }
            };
        }    // namespace block
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_CIPHERS_BASIC_SHACAL_HPP


/* Copyright (c) 2020 C. Pattison
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once
#include <cassert>
#include <algorithm>
#include <utility>
#include <complex>
#include <random>
#include <exception>
#include <cmath>

#include <Eigen/Dense>
#include "syk_types.hpp"
#include "pauli.hpp"
#include "util.hpp"

/** Generation for SYK Hamiltonians
 */

namespace syk {
using namespace std::complex_literals;

/** Utility class for construction Majorana fermion representations
 */
struct FermionRep : PauliRep {
protected:
    int num_fermions_;

public:

    FermionRep(int num_fermions) : PauliRep(num_fermions/2), num_fermions_(num_fermions) {
        if(!(num_fermions > 0 && num_fermions % 2 == 0)) { throw std::runtime_error("Number of fermions must be positive and even"); }
    }

    Pauli get_pauli_factor(int a, int qubit_index) {
        assert(0 <= a && a < num_fermions_);
        assert(0 <= qubit_index && qubit_index < num_qubits_);
        if (a/2 == qubit_index) { return (a%2 == 1) ? PauliY() : PauliX(); }
        if (a/2 < qubit_index)  { return PauliI(); }
        if (a/2 > qubit_index)  { return PauliZ(); }

        assert(false);
        return PauliI();
    }

    /** Four body interaction terms
     */
    PauliString four_fermion(int a, int b, int c, int d) {
        PauliString pstring;
        for(int qubit_index = 0; qubit_index < num_qubits_; ++qubit_index) {
            pstring[qubit_index] =
                  get_pauli_factor(a, qubit_index)
                * get_pauli_factor(b, qubit_index)
                * get_pauli_factor(c, qubit_index)
                * get_pauli_factor(d, qubit_index);
        }
        return pstring;
    }

    /** Returns the parity operator (gamma_*) for the Clifford algebra repp
     */
    PauliString gamma_star() const {
        PauliString pstring;
        pstring.fill(PauliZ());
        return pstring;
    }
};

/** Return all numbers of even hamming weight below size
 */
std::vector<std::uint64_t> hamming_weight(std::uint64_t size, bool odd = false) {

    std::vector<std::uint64_t> idx;
    idx.reserve(size/2);
    for (std::uint64_t i = 0; i < size; ++i) {
        if (static_cast<bool>(__builtin_popcount(i) % 2) == odd) {
            idx.push_back(i);
        }
    }

    return idx;
}

/** Generate an SYK Hamiltonian
 * Returns only a single parity sector
 */
template<typename rng_type>
__attribute__((optimize("fast-math")))
MatrixType syk_hamiltonian(rng_type* rng, int N, double J) {
    auto hilbert_space_size = (1 << (N+1)/2);
    auto repp = FermionRep(N);

    auto distr = std::normal_distribution<double>(0, J/std::pow(static_cast<double>(N), 1.5));

    std::vector<FermionRep::Term> interactions;
    interactions.reserve(N*N*N);
    for(int i = 0; i < N; ++i) {
        for(int j = 0; j < i; ++j) {
            for(int k = 0; k < j; ++k) {
                for(int l = 0; l < k; ++l) {
                    interactions.push_back(FermionRep::Term { repp.four_fermion(i, j, k, l), distr(*rng)} );
                }
            }
        }
    }

    // The parity projection (1 + gamma_*) removes all rows/columns with odd hamming weight
    auto even_weight = hamming_weight(hilbert_space_size);
    auto hamiltonian = repp.get_hamiltonian(interactions, even_weight);

    return hamiltonian;
}

/** Returns a lambda for computing the spectral form factor at some time
 */
auto spectral_form_factor(const std::vector<double>& eigenvals) {
    return [=](std::complex<double> beta) -> double {
        auto z_part = util::transform_reduce(eigenvals.cbegin(), eigenvals.cend(), 0.0i,
            std::plus<>(), [=](auto a) { return std::exp(a * beta); });
        return std::real(z_part * std::conj(z_part));
    };
}

/** Vector of doubles to Eigen::VectorXd
 */
auto to_eigen_vector(const std::vector<double>& vals) {
    Eigen::Matrix<MatrixType::Scalar, Eigen::Dynamic, 1> vec(vals.size());
    std::copy(vals.begin(), vals.end(), vec.data());
    return vec;
}
}


#include "pinocchio/multibody/model.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/frames.hpp"

#include "sot/core/parameter-server.hh"
#include "sot/talos_balance/distribute-wrench.hh"

#include <boost/test/unit_test.hpp>

#include "test-paths.h"

using namespace dynamicgraph::sot;
using namespace dynamicgraph::sot::talos_balance;

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_distribute )
{
  // --- General ---
  std::cout << "--- General ---" << std::endl;

  const double dt = 0.001;
  const std::string robot_name = "robot";

  Eigen::VectorXd q(39);

  q <<  0.0,  0.0,  1.018213,  0.0,  0.0, 0.0, 1.0,                     //Free flyer
        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.0,                  //Left Leg
        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.0,                  //Right Leg
        0.0,  0.006761,                                                    //Chest
        0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1, -0.005,  //Left Arm
       -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,  //Right Arm
        0.,  0.;                                                           //Head

  std::cout << "q: " << q.transpose() << std::endl;

  std::string urdfPath = TALOS_DATA_MODEL_DIR "urdf/talos_reduced.urdf";

  pinocchio::Model model;
  pinocchio::urdf::buildModel(urdfPath, pinocchio::JointModelFreeFlyer(), model);
  pinocchio::Data data(model);
  Eigen::Vector3d com = pinocchio::centerOfMass(model,data,q);
  pinocchio::updateFramePlacements(model,data);
  double m = data.mass[0];
  //com[1] = 0. # ensure perfect symmetry

  std::cout << "com: " << com.transpose() << std::endl;

  const std::string leftName = "leg_left_6_joint";
  const pinocchio::FrameIndex leftId = model.getFrameId(leftName);
  pinocchio::SE3 leftPos  = data.oMf[leftId];
  std::cout << leftName << ": " << leftId << std::endl;
  std::cout << leftPos << std::endl;

  const std::string rightName = "leg_right_6_joint";
  const pinocchio::FrameIndex rightId = model.getFrameId(rightName);
  pinocchio::SE3 rightPos = data.oMf[rightId];
  // Eigen::Vector3d pR = leftPos.translation // ensure perfect symmetry
  // pR[1] = -pR[1]
  // rightPos.translation = pR
  std::cout << rightName << ": " << rightId << std::endl;
  std::cout << rightPos << std::endl;

  double g = 9.81;
  double fz = m*g;
  Eigen::Vector3d force(0.0, 0.0, fz);
  double lx = com[0];
  double tauy = -fz*lx;
  Eigen::Vector3d moment(0.0, tauy,   0.0);
  Eigen::VectorXd wrench(6);
  wrench << force, moment;

  std::cout << "total wrench: " << wrench.transpose() << std::endl;

  // --- Parameter server ---
  std::cout << "--- Parameter server ---" << std::endl;

  ParameterServer param_server("param_server");

  param_server.init(dt, urdfPath, robot_name);

  // Set the map from joint name to joint ID
  // for key in conf.mapJointNameToID:
  //      param_server.setNameToId(key, conf.mapJointNameToID[key]);

//    # Set the map joint limits for each id
//    for key in conf.mapJointLimits:
//        param_server.setJointLimitsFromId(key, conf.mapJointLimits[key][0],
//                                          conf.mapJointLimits[key][1])

//    # Set the force limits for each id
//    for key in conf.mapForceIdToForceLimits:
//        param_server.setForceLimitsFromId(key, tuple(conf.mapForceIdToForceLimits[key][0]),
//                                          tuple(conf.mapForceIdToForceLimits[key][1]))

    // Set the force sensor id for each sensor name
//    for key in conf.mapNameToForceId:
//        param_server.setForceNameToForceId(key, conf.mapNameToForceId[key])

//    # Set the map from the urdf joint list to the sot joint list
//    param_server.setJointsUrdfToSot(conf.urdftosot)

  // Set the foot frame name
  param_server.setFootFrameName("Right", "leg_right_6_joint");
  param_server.setFootFrameName("Left" , "leg_left_6_joint");

//    # Set IMU hosting joint name
//    param_server.setImuJointName(conf.ImuJointName)

  Eigen::VectorXd rightFootSensorXYZ(3);
  rightFootSensorXYZ << 0.0,0.0,-0.085;
  param_server.setRightFootForceSensorXYZ(rightFootSensorXYZ);

  Eigen::VectorXd rightFootSoleXYZ(3);
  rightFootSoleXYZ << 0.0,0.0,-0.105;

  param_server.setRightFootSoleXYZ(rightFootSoleXYZ);

  // --- DistributeWrench ---
  std::cout << "--- DistributeWrench ---" << std::endl;

  DistributeWrench distribute("distribute");

  distribute.m_phaseSIN.setConstant(0);
  distribute.m_rhoSIN.setConstant(0.5);

  Eigen::VectorXd RIGHT_FOOT_SIZES(4);
  RIGHT_FOOT_SIZES << 0.100, -0.100, 0.06, -0.06;  // pos x, neg x, pos y, neg y size
  Eigen::VectorXd LEFT_FOOT_SIZES(4);
  LEFT_FOOT_SIZES  << 0.100, -0.100, 0.06, -0.06;  // pos x, neg x, pos y, neg y size

  const double minPressure = 15.;
  const double frictionCoefficient = 0.7;

  const double wSum   = 10000.0;
  const double wNorm  = 10.0;
  const double wRatio = 1.0;
  Eigen::VectorXd wAnkle(6);
  wAnkle << 1., 1., 1e-4, 1., 1., 1e-4;

  distribute.m_eps = minPressure; // setMinPressure(minPressure);
  distribute.m_frictionCoefficientSIN.setConstant(frictionCoefficient);
  distribute.m_wSumSIN.setConstant(wSum);
  distribute.m_wNormSIN.setConstant(wNorm);
  distribute.m_wRatioSIN.setConstant(wRatio);
  distribute.m_wAnkleSIN.setConstant(wAnkle);

  distribute.set_right_foot_sizes(RIGHT_FOOT_SIZES);
  distribute.set_left_foot_sizes(LEFT_FOOT_SIZES);

  distribute.m_qSIN.setConstant(q);
  distribute.m_wrenchDesSIN.setConstant(wrench);

  distribute.init(robot_name);

  // --- Wrench distribution ---
  std::cout << std::endl;
  std::cout << "--- Wrench distribution ---" << std::endl;
  distribute.m_phaseSIN.setConstant(0);

  Eigen::Vector3d forceLeft(0.0, 0.0, fz/2);
  Eigen::Vector3d forceRight(0.0, 0.0, fz/2);
  const double ly = leftPos.translation()[1];
  const double taux = fz*ly/2;
  Eigen::Vector3d momentLeft(  taux, tauy/2, 0.0);
  Eigen::Vector3d momentRight(-taux, tauy/2, 0.0);

  Eigen::VectorXd wrenchLeft(6);
  wrenchLeft << forceLeft, momentLeft;
  Eigen::VectorXd wrenchRight(6);
  wrenchRight << forceRight, momentRight;

  lx = com[0]-leftPos.translation()[0];
  tauy = -fz*lx/2;
  Eigen::Vector3d ankleMomentLeft( 0.0, tauy, 0.0);
  Eigen::Vector3d ankleMomentRight(0.0, tauy, 0.0);

  Eigen::VectorXd ankleWrenchLeft(6);
  ankleWrenchLeft << forceLeft, ankleMomentLeft;
  Eigen::VectorXd ankleWrenchRight(6);
  ankleWrenchRight << forceRight, ankleMomentRight;

  std::cout << "expected global wrench:       " << wrench.transpose() << std::endl;
  std::cout << "expected global left wrench:  " << wrenchLeft.transpose() << std::endl;
  std::cout << "expected global right wrench: " << wrenchRight.transpose() << std::endl;
  std::cout << "expected ankle left wrench:   " << ankleWrenchLeft.transpose() << std::endl;
  std::cout << "expected ankle right wrench:  " << ankleWrenchRight.transpose() << std::endl;

  Eigen::Vector3d copLeft(com[0] - leftPos.translation()[0],  0., 0.);
  Eigen::Vector3d copRight(com[0] - rightPos.translation()[0], 0., 0.);

  std::cout << "expected sole left CoP: "  << copLeft.transpose()  << std::endl;
  std::cout << "expected sole right CoP: " << copRight.transpose() << std::endl;
  std::cout << std::endl;

  distribute.m_zmpRefSOUT.recompute(0);

  std::cout << "resulting global wrench: " << distribute.m_wrenchRefSOUT(0).transpose() << std::endl;
  BOOST_CHECK(wrench.isApprox(distribute.m_wrenchRefSOUT(0),1e-3));

  std::cout << "resulting global left wrench:  " << distribute.m_wrenchLeftSOUT(0).transpose() << std::endl;
  BOOST_CHECK(wrenchLeft.isApprox(distribute.m_wrenchLeftSOUT(0),1e-3));
  std::cout << "resulting global right wrench: " << distribute.m_wrenchRightSOUT(0).transpose() << std::endl;
  BOOST_CHECK(wrenchRight.isApprox(distribute.m_wrenchRightSOUT(0),1e-3));

  distribute.m_ankleWrenchLeftSOUT.recompute(0);
  distribute.m_ankleWrenchRightSOUT.recompute(0);

  std::cout << "resulting ankle left wrench:  " << distribute.m_ankleWrenchLeftSOUT(0).transpose() << std::endl;
  BOOST_CHECK(ankleWrenchLeft.isApprox(distribute.m_ankleWrenchLeftSOUT(0),1e-3));
  std::cout << "resulting ankle right wrench: " << distribute.m_ankleWrenchRightSOUT(0).transpose() << std::endl;
  BOOST_CHECK(ankleWrenchRight.isApprox(distribute.m_ankleWrenchRightSOUT(0),1e-3));

  distribute.m_copLeftSOUT.recompute(0);
  distribute.m_copRightSOUT.recompute(0);

  std::cout << "resulting sole left CoP:  " << distribute.m_copLeftSOUT(0).transpose() << std::endl;
  BOOST_CHECK(copLeft.isApprox(distribute.m_copLeftSOUT(0),1e-3));
  std::cout << "resulting sole right CoP: " << distribute.m_copRightSOUT(0).transpose() << std::endl;
  BOOST_CHECK(copRight.isApprox(distribute.m_copRightSOUT(0),1e-3));

  distribute.m_emergencyStopSOUT.recompute(0);
  bool stop = distribute.m_emergencyStopSOUT(0);
  BOOST_CHECK(!stop);

  // --- Wrench saturation ---
  std::cout << std::endl;
  std::cout << "--- Wrench saturation ---" << std::endl;
  std::cout << "NOTE: \"predicted\" wrench values are not accurate due to the foot saturation and as such they are not checked." << std::endl;
  std::cout << "CoP values are predicted under the assumption that they are at the foot border and as such they are checked." << std::endl;

  // --- Wrench saturation (left) ---
  std::cout << std::endl;
  std::cout << "--- Wrench saturation (left) ---" << std::endl;
  distribute.m_phaseSIN.setConstant(1);
  distribute.m_phaseSIN.setTime(1);

  wrenchLeft  = wrench;
  ankleWrenchLeft  = leftPos.actInv(pinocchio::Force(wrenchLeft)).toVector();

  std::cout << "expected global wrench: " << wrench.transpose() << std::endl;
  std::cout << "expected global left wrench: " << wrenchLeft.transpose() << std::endl;
  std::cout << "expected ankle left wrench: " << ankleWrenchLeft.transpose() << std::endl;

  copLeft << com[0] - leftPos.translation()[0], RIGHT_FOOT_SIZES[3], 0.;

  std::cout << "expected sole left CoP: " << copLeft.transpose() << std::endl;
  std::cout << std::endl;

  distribute.m_zmpRefSOUT.recompute(1);

  std::cout << "resulting global wrench: " << distribute.m_wrenchRefSOUT(1).transpose() << std::endl;
  //BOOST_CHECK(wrench.isApprox(distribute.m_wrenchRefSOUT(1)));
  std::cout << "resulting global left wrench: " << distribute.m_wrenchLeftSOUT(1).transpose() << std::endl;
  //BOOST_CHECK(wrenchLeft.isApprox(distribute.m_wrenchLeftSOUT(1)));

  distribute.m_ankleWrenchLeftSOUT.recompute(1);

  std::cout << "resulting ankle left wrench: " << distribute.m_ankleWrenchLeftSOUT(1).transpose() << std::endl;
  //BOOST_CHECK(ankleWrenchLeft.isApprox(distribute.m_ankleWrenchLeftSOUT(1)));

  distribute.m_copLeftSOUT.recompute(1);
  distribute.m_copRightSOUT.recompute(1);

  std::cout << "resulting sole left CoP: " << distribute.m_copLeftSOUT(1).transpose() << std::endl;
  BOOST_CHECK(copLeft.isApprox(distribute.m_copLeftSOUT(1),1e-4));

  distribute.m_emergencyStopSOUT.recompute(1);
  stop = distribute.m_emergencyStopSOUT(1);
  BOOST_CHECK(!stop);

  // --- Wrench saturation (right) ---
  std::cout << std::endl;
  std::cout << "--- Wrench saturation (right) ---" << std::endl;
  distribute.m_phaseSIN.setConstant(-1);
  distribute.m_phaseSIN.setTime(2);

  wrenchRight = wrench;
  ankleWrenchRight = rightPos.actInv(pinocchio::Force(wrenchRight)).toVector();

  std::cout << "expected global wrench: " << wrench.transpose() << std::endl;
  std::cout << "expected global right wrench: " << wrenchRight.transpose() << std::endl;
  std::cout << "expected ankle right wrench: " << ankleWrenchRight.transpose() << std::endl;

  copRight << com[0] - rightPos.translation()[0], RIGHT_FOOT_SIZES[2], 0.;

  std::cout << "expected sole right CoP: " << copRight.transpose() << std::endl;
  std::cout << std::endl;

  distribute.m_zmpRefSOUT.recompute(2);

  std::cout << "resulting global wrench: " << distribute.m_wrenchRefSOUT(2).transpose() << std::endl;
  //BOOST_CHECK(wrench.isApprox(distribute.m_wrenchRefSOUT(2)));
  std::cout << "resulting global right wrench: " << distribute.m_wrenchRightSOUT(2).transpose() << std::endl;
  //BOOST_CHECK(wrenchRight.isApprox(distribute.m_wrenchRightSOUT(2)));

  distribute.m_ankleWrenchRightSOUT.recompute(2);

  std::cout << "resulting ankle right wrench: " << distribute.m_ankleWrenchRightSOUT(2).transpose() << std::endl;
  //BOOST_CHECK(ankleWrenchRight.isApprox(distribute.m_ankleWrenchRightSOUT(2)));

  distribute.m_copLeftSOUT.recompute(2);
  distribute.m_copRightSOUT.recompute(2);

  std::cout << "resulting sole right CoP: " << distribute.m_copRightSOUT(2).transpose() << std::endl;
  BOOST_CHECK(copRight.isApprox(distribute.m_copRightSOUT(2),1e-4));

  distribute.m_emergencyStopSOUT.recompute(2);
  stop = distribute.m_emergencyStopSOUT(2);
  BOOST_CHECK(!stop);

}

BOOST_AUTO_TEST_SUITE_END ()