/* Copyright (C) 2017 Karl Phillip Buhr <karlphillip@gmail.com>
 *
 * This work is licensed under the MIT License.
 * To view a copy of this license, visit:
 *      https://opensource.org/licenses/MIT
 *
 * This file is part of Madplotlib, a C++ library for building simple
 * 2D plots inspired on matplotlib.
 */
/*
 * plot() has many forms. This file demonstrates how to use some of them.
 * However, the most complete form of plot() currently is:
 *     plot(x, y, marker, label, alpha, color, linewidth, markeredgecolor, markersize)
 *
 * Happy plotting!
 */

#include <Eigen/Dense>

#include "Madplotlib.h"

#include <QApplication>

// Uncomment the line below to save each chart as PNG image
#define SCRSHOT

/* Use case: simple line chart.
 * plot() draws the values of x and y on a line chart.
 * show() creates a new window to display the chart.
 * savefig() saves a screenshot of the chart as PNG image.
 */
void test1()
{
    Eigen::ArrayXf x(16);
    x <<   0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7,
         0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5;

    Eigen::ArrayXf y(16);
    y <<  65,  79,  80,  68,  77,  81, 100, 102,
         105, 111, 120, 126, 120, 104,  85,  92;

    Madplotlib plt;
    plt.plot(x, y);
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test1.png");
#endif
}

/* Use case: simple scatter chart.
 * plot() draws the values of x and y on a scatter chart.
 * show() creates a new window to display the chart.
 */
void test2()
{
    float period = 0.5;
    Eigen::ArrayXf x = Eigen::ArrayXf::LinSpaced(72, 0, 20);
    Eigen::ArrayXf y = x.cos() * period;

    Madplotlib plt;
    plt.title("Test 2: Simple Scatter Plot");
    plt.plot(x, y, QString("o"));
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test2.png");
#endif
}

/* Use case: plot 3 data sets on a line chart.
 * + axis() defines the X,Y range to make the labels look cooler.
 * + plot() draws X and Y as a continuous line, thicker (linewidth = 4).
 * + plot() adds 5 to every element if the Y axis and then draws it as a dashed line.
 * + plot() adds 10 to every element if the Y axis and then draws it as a dotted line.
 * + legend() with no args, displays the labels passed to plot() at a default position.
 */
void test3()
{
    float period = 2;
    Eigen::ArrayXf x = Eigen::ArrayXf::LinSpaced(100, 0, 25);
    Eigen::ArrayXf y = x.cos() * period;

    Madplotlib plt;
    plt.title("Test 3: Multiple Data Series");
    plt.axis(0, 25, 0, 14);
    plt.plot(x, y, marker=QString("--"), label=QString("label=Dashed Line"));
    plt.plot(x, y+5, label=QString("label=Default Line"), (quint32)4);
    plt.plot(x, y+10, QString("."), label=QString("label=Dotted Line"));
    plt.legend(); // default position is "lower center"
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test3.png");
#endif
}

/* Use case: simple scatter chart.
 * + Generates 50 random values between [0, 50] for the first data series.
 * + Generates 72 random values between [0, 75] for the second data series.
 * + title()
 * + locator_params() displays 10 ticks on the X axis.
 * + axis() defines the range of values for each axis.
 * + plot() uses the parameter "o" for a scatter plot,
 *          changes the transparency level to 30% for a red color marker,
 *          with a markersize of 8.
 * + plot() uses the parameter "o" for a scatter plot,
 *          changes the transparency level to 50% for a blue color marker.
 */
void test4()
{
    Eigen::ArrayXf x_rand = Eigen::ArrayXf::Random(64) * 50;
    Eigen::ArrayXf x = x_rand.abs();

    Eigen::ArrayXf y_rand = Eigen::ArrayXf::Random(64) * 50;
    Eigen::ArrayXf y = y_rand.abs();

    Eigen::ArrayXf x_rand2 = Eigen::ArrayXf::Random(72) * 75;
    Eigen::ArrayXf x2 = x_rand2.abs();

    Eigen::ArrayXf y_rand2 = Eigen::ArrayXf::Random(72) * 75;
    Eigen::ArrayXf y2 = y_rand2.abs();

    Madplotlib plt;
    plt.title("Test 4: Random Scatter Plot");
    plt.locator_params("x", 10);
    plt.axis(-25, 100, -25, 100);
    plt.plot(x, y, marker=QString("o"), 0.7f, QColor(255, 0, 0), 8.0f); // red, 30% transparent, markersize 8
    plt.plot(x2, y2, marker=QString("o"), 0.5f, QColor(0, 0, 255));     // blue, 50% transparent
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test4.png");
#endif
}

/* Use case that defines 3 series of data containing only Y axis values.
 * + title()
 * + ylabel()
 * + xlabel()
 * + plot() automatically creates data for the X axis and labels it Linear.
 * + plot() automatically creates data for the X axis and labels it Exponential.
 * + plot() automatically creates data for the X axis and labels it Flat.
 * + legend() displays all labels set through plot() in a legend positioned at the right.
 */
void test5()
{
    // linear
    Eigen::ArrayXf a = Eigen::ArrayXf::LinSpaced(20, 0, 2000);

    // exponential
    Eigen::ArrayXf b = Eigen::ArrayXf::LinSpaced(20, 0, 100);
    b = b * b;

    // flat
    Eigen::ArrayXf c = Eigen::ArrayXf::Zero(20);
    c = 1000;

    Madplotlib plt;
    plt.title("Test 5: Linear vs Exponential vs Flat");
    plt.ylabel("Y Values");
    plt.xlabel("X Values");

    plt.plot(a, QString("label=Linear"));
    plt.plot(b, QString("label=Exponential"));
    plt.plot(c, QString("label=Flat"));
    plt.legend("loc=center right");
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test5.png");
#endif
}

/* Use case that plots 4 data sets.
 * + plot() argument "o" defines a scatter plot.
 * + plot() argument QColor defines the color for the line plot.
 * + xlabel() defines the label for the X axis.
 * + ylabel() defines the label for the Y axis.
 */
void test6()
{
    Eigen::ArrayXf x(16);
    x <<   0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7,
         0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5;

    Eigen::ArrayXf y(16);
    y <<  65,  79,  80,  68,  77,  81, 100, 102,
         105, 111, 120, 126, 120, 104,  85,  92;

    Madplotlib plt;
    plt.plot(x, y, color=QColor(0xFF2700));               // red
    plt.plot(x, y, marker=QString("o"), color=QColor(0xFF2700));
    plt.plot(x, y-40, color=QColor(0x008FD5));            // blue
    plt.plot(x, y-40, marker=QString("o"), color=QColor(0x008FD5));

    plt.title("Test 6: Line + Scatter");
    plt.xlabel("X values");
    plt.ylabel("Y values");
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test6.png");
#endif
}

/* Use case with several UI customizations.
 * + title()
 * + xlabel() defines the label for the X axis.
 * + ylabel() defines the label for the Y axis.
 * + yticks() changes the default categories of Y axis for customized ones:
 *      Low:    represents all data between [0, 1]
 *      High:   represents all data between [1, 2]
 * + ylim() defines the range for the Y axis.
 * + plot() draws a scatter plot with larger circles (markersize = 7.0).
 * + grid() enables drawing of the background grid.
 */
void test7()
{
    const double pi = std::acos(-1.0);

    Eigen::ArrayXf x = Eigen::ArrayXf::LinSpaced(64, 0.0, 5.0);
    Eigen::ArrayXf y = Eigen::cos(2 * pi * x) * Eigen::exp(-x);

    Eigen::ArrayXf y_ticks(2);
    y_ticks << 1, 2;

    QVector<QString> y_labels;
    y_labels.push_back("Low");
    y_labels.push_back("High");

    Madplotlib plt;
    plt.title("Test 7: UI Customizations: Grid & Categories");
    plt.xlabel("time (s)");
    plt.ylabel("voltage (mV)");
    plt.yticks(y_ticks, y_labels);

    // On Qt 7.5, Qt Charts has a bug spacing correctly categories on negative Y axis (-1, 1).
    // For now, drawing (y+1) will bypass that since the values will fall between (0, 2).
    plt.ylim(0, 2);
    plt.plot(x, y+1, marker=QString("o"), alpha=1.f, linewidth=2, markersize=7.0f); // alpha=1.f, linewidth=2, markersize=7.f
    plt.grid(true);

    plt.show();

#ifdef SCRSHOT
    plt.savefig("test7.png");
#endif
}

/* Use case that displays two waves on a scatter plot and then connects them with lines.
 * + axis() param "off" hides both axis ticks.
 * + plot() draw the X series using x's squared root as the Y series.
 * + plot() draw the X series using x's negative squared root as the Y series.
 * + noise is an array with 50 random numbers between [0, 2].
 * + plot() draw the X series using the difference of x's squared root and the noise asthe Y series.
 * + plot() draw the X series using the difference of x's negative squared root and the noise as the Y series.
 */
void test8()
{
    Eigen::ArrayXf x = Eigen::ArrayXf::LinSpaced(50, 0, 100);

    Madplotlib plt;
    plt.title("Test 8: Line + Square Markers + Hidden Ticks");
    plt.axis("off");
    plt.plot(x, x.sqrt(), color=QColor(0, 0, 0));
    plt.plot(x, -x.sqrt(), color = QColor(0, 0, 0));

    Eigen::ArrayXf noise = Eigen::ArrayXf::Random(50) * 2;

    plt.plot(x, x.sqrt() - noise, marker=QString("s"), alpha = 0.7f, color=QColor(19, 154, 255), edgecolor=QColor(19, 154, 255)); // red squares without black edges
    plt.plot(x, -x.sqrt() - noise, marker = QString("s"), alpha = 0.7f, color=QColor(255, 41, 5), edgecolor=QColor(255, 41, 5));    // blue squares without black edges
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test8.png");
#endif
}

/* Use case that displays Y data twice with custom limits and ticks for both axis.
 * + title()
 * + xlabel()
 * + ylabel()
 * + plot() draws X,Y using a specific color.
 * + plot() uses circular markers with green edges filled with white.
 */
void test9()
{
    Eigen::ArrayXf x(26);
    x << 0, 102, 205, 301, 404,
         500, 601, 701, 804, 904,
         1006, 1503, 2016, 3086, 4002,
         5178, 10094, 16012, 21267, 25985,
         32641, 48486, 54925, 58145, 63291,
         98029;

    Eigen::ArrayXf y(26);
    y << 924, 794, 2708, 3324, 5037,
         3849, 6150, 5975, 9275, 5410,
         9222, 10592, 12374, 22348, 27508,
         18361, 39910, 31354, 36074, 20413,
         69383, 252988, 12457, 48495, 171303,
         69783;

    Madplotlib plt;
    plt.title("Test 9: Fox News Facebook Shares vs Likes");
    plt.xlabel("Fox News Shares");
    plt.ylabel("Fox News Likes");

    plt.plot(x, y, QColor(169, 206, 0)); // plot green line
    plt.plot(x, y, QString("o"), QColor(255, 255, 255), 2, QColor(169, 206, 0), 6.5f); // plot markers, linewith=2, markersize=6.5
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test9.png");
#endif
}

/* Use case that displays Y data twice and hides the labels from X axis.
 * + plot() draws Y data as a thicker red line (linewidth = 4).
 * + plot() draws Y data as scattered with larger colored markers (markersize = 15.0f).
 */
void test10()
{
    const double pi = std::acos(-1.0);
    Eigen::ArrayXf X = Eigen::ArrayXf::LinSpaced(256, 0, 2*pi);
    Eigen::ArrayXf C = Eigen::cos(X);
    Eigen::ArrayXf S = Eigen::sin(X);

    Madplotlib plt;
    plt.title("Test 10");
    plt.grid(true);

    Eigen::ArrayXf x_ticks(4);
    x_ticks << pi/2, pi, (3*pi)/2, 2*pi;

    QVector<QString> x_labels;
    x_labels.push_back("π/2");
    x_labels.push_back("π");
    x_labels.push_back("3π/2");
    x_labels.push_back("2π");

    // On Qt 7.5, Qt Charts has a bug spacing correctly categories on negative X axis (-3.1, 3.1).
    // For now, make sure you are using only positive X values to bypass that problem.
    plt.plot(X, C);
    plt.plot(X, S, marker=QString("--"));
    plt.xlim(0, 2*pi);
    plt.xticks(x_ticks, x_labels);
    plt.show();

#ifdef SCRSHOT
    plt.savefig("test10.png");
#endif
}

void run_test(int id)
{
    if (id == 0 || id == 1)
        test1();

    if (id == 0 || id == 2)
        test2();

    if (id == 0 || id == 3)
        test3();

    if (id == 0 || id == 4)
        test4();

    if (id == 0 || id == 5)
        test5();

    if (id == 0 || id == 6)
        test6();

    if (id == 0 || id == 7)
        test7();

    if (id == 0 || id == 8)
        test8();

    if (id == 0 || id == 9)
        test9();

    if (id == 0 || id == 10)
        test10();
}

void run_test(int begin, int end)
{
    for (int i = begin; i <= end; i++)
        run_test(i);
}

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

    // runtest(0)    - executes all tests.
    // runtest(5, 9) - executes all tests between 5 and 9.
    run_test(0);

    qInfo() << "* Done!";

    // NOTE:
    // Creating an object with the default constructor makes the window
    // block/freeze your program execution upon show().
    // When the user closes the window, your program continues to run
    // normally until the next show() is called.
    // This is how matplotlib behaves!
    //
    // However, if you don't want Madplotlib to block you need
    // to create a Madplotlib object and pass TRUE to the constructor.
    // This will make Madplotlib behave like a traditional widget.
    // This is what you need to do if you plan to write custom Qt GUIs
    // with widgets. Also, don't forget to invoke app.exec().

    return 0;
}


/*
    Author: hauptmech <hauptmech@gmail.com>, Nov 2013

    This is free and unencumbered software released into the public domain.

    Anyone is free to copy, modify, publish, use, compile, sell, or
    distribute this software, either in source code form or as a compiled
    binary, for any purpose, commercial or non-commercial, and by any
    means.

    In jurisdictions that recognize copyright laws, the author or authors
    of this software dedicate any and all copyright interest in the
    software to the public domain. We make this dedication for the benefit
    of the public at large and to the detriment of our heirs and
    successors. We intend this dedication to be an overt act of
    relinquishment in perpetuity of all present and future rights to this
    software under copyright law.

    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 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.

    For more information, please refer to <http://unlicense.org/>
*/

#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main()
{
    // Init a fixed size 3x3 matrix
    Matrix3d m3 {{1.2,2.2,3.3},{4.2,2.5,6.3},{7.2,8.2,9.3}};
    std::cout << "\nm3:\n" << m3 << std::endl;

    // Initialize a variable length Matrix to 2x2
    MatrixXd mY {{1.2,2.2},{3.2,4.5}};
    std::cout << "\nmY:\n" << mY << std::endl;

    // Initialize a variable length Matrix (Vector) to 9x1
    // 2D Matrices are initialized column-wise
    MatrixXd mX {1.2,2.2,3.3, 4.2,2.5,6.3, 7.2,8.2,9.3};
    std::cout << "\nmX:\n" << mX << std::endl;

    // Init a 3 element vector
    Vector3d v3  {1,3,3};
    std::cout << "\nv3:\n" << v3 << std::endl;

    // Init a fixed size 3x3 array
    Array33d a3 {{1.2,2.2,3.3},{4.2,2.5,6.3},{7.2,8.2,9.3}};
    std::cout << "\na3:\n" << a3 << std::endl;

    // Initialize a variable length Array to 2x2
    ArrayXXd aY {{1.2,2.2},{3.2,4.5}};
    std::cout << "\naY:\n" << aY << std::endl;

    // Initialize a variable length Array to 9x1
    // 2D Arrays are initialized column-wise
    ArrayXXd aX {1.2,2.2,3.3, 4.2,2.5,6.3, 7.2,8.2,9.3};
    std::cout << "\naX:\n" << aX << std::endl;

    // Init a 3 element array
    Array3d w3  {1,3,3};
    std::cout << "\nw3:\n" << w3 << std::endl;
}


//  (C) Copyright Gennadiy Rozental 2011-2015.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

BOOST_AUTO_TEST_CASE( test )
{
  double v1 = 1.23456e-10;
  double v2 = 1.23457e-10;

  BOOST_CHECK_CLOSE( v1, v2, 0.0001 );
  // Absolute value of difference between these two values is 1e-15. They seems
  // to be very close. But we want to checks that these values differ no more then 0.0001%
  // of their value. And this test will fail at tolerance supplied.
}
//]


/**
 * @file
 * @brief NPDE homework "Handling degrees of freedom (DOFs) in LehrFEM++"
 * @author Julien Gacon
 * @date March 1st, 2019
 * @copyright Developed at ETH Zurich
 */

#include "lfppdofhandling.h"

#include <Eigen/Dense>
#include <array>
#include <memory>

#include "lf/assemble/assemble.h"
#include "lf/base/base.h"
#include "lf/geometry/geometry.h"
#include "lf/mesh/mesh.h"
#include "lf/mesh/utils/utils.h"

namespace LFPPDofHandling {

/* SAM_LISTING_BEGIN_1 */
std::array<std::size_t, 3> countEntityDofs(
    const lf::assemble::DofHandler &dofhandler) {
  std::array<std::size_t, 3> entityDofs;
  //====================
  // Your code goes here
  //====================
  for (int codim=0; codim<3; codim++){
    entityDofs[codim] = 0;
  }

  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();

  for (int codim=0; codim<3; codim++){
    for (const auto *entity : mesh->Entities(codim)){
      if (entity->RefEl() == lf::base::RefEl::kQuad()){
        throw("Error");
      }
      entityDofs[codim] += dofhandler.NumInteriorDofs(*entity);
    }
  }
  return entityDofs;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::size_t countBoundaryDofs(const lf::assemble::DofHandler &dofhandler) {
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  // given an entity, bd\_flags(entity) == true, if the entity is on the
  // boundary
  lf::mesh::utils::AllCodimMeshDataSet<bool> bd_flags(
      lf::mesh::utils::flagEntitiesOnBoundary(mesh));
  std::size_t no_dofs_on_bd = 0;
  //====================
  // Your code goes here
  //====================

  for (const auto *vertex : mesh->Entities(2)){
    if (bd_flags(*vertex)){
      no_dofs_on_bd+=1;
    }
  }
  return no_dofs_on_bd;
}
/* SAM_LISTING_END_2 */

// clang-format off
/* SAM_LISTING_BEGIN_3 */
double integrateLinearFEFunction(
    const lf::assemble::DofHandler& dofhandler,
    const Eigen::VectorXd& mu) {
  double I = 0;
  //====================
  // Your code goes here
  //====================

  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  double area;
  for (const auto *cell : mesh->Entities(0)){
    lf::base::size_type cell_local_dofs = dofhandler.NumLocalDofs(*cell);
    if (cell_local_dofs!=3){
      throw("Error");
    }
    lf::geometry::Geometry *cell_geo = cell->Geometry();
    area = lf::geometry::Volume(*cell_geo);
    const auto global_idxs = dofhandler.GlobalDofIndices(*cell);
    for (auto idx_p=global_idxs.begin() ; idx_p<global_idxs.end() ; ++idx_p){
      I += area/3.0 * mu(*idx_p);
    }
  }
  return I;
}
/* SAM_LISTING_END_3 */
// clang-format on

/* SAM_LISTING_BEGIN_4 */
double integrateQuadraticFEFunction(const lf::assemble::DofHandler &dofhandler,
                                    const Eigen::VectorXd &mu) {
  double I = 0;
  //====================
  // Your code goes here
  //====================
  //
  std::shared_ptr<const lf::mesh::Mesh> mesh = dofhandler.Mesh();
  double area;
  for (const auto *cell : mesh->Entities(0)){
    lf::base::size_type cell_local_dofs = dofhandler.NumLocalDofs(*cell);
    if (cell_local_dofs!=6){
      throw("Error");
    }
    lf::geometry::Geometry *cell_geo = cell->Geometry();
    area = lf::geometry::Volume(*cell_geo);
    const auto global_idxs = dofhandler.GlobalDofIndices(*cell);
    for (int i=3; i<6; i++){
      I += (area/3.0 * mu[global_idxs[i]]);
    }
  }
  return I;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::VectorXd convertDOFsLinearQuadratic(
    const lf::assemble::DofHandler &dofh_Linear_FE,
    const lf::assemble::DofHandler &dofh_Quadratic_FE,
    const Eigen::VectorXd &mu) {
  if (dofh_Linear_FE.Mesh() != dofh_Quadratic_FE.Mesh()) {
    throw "Underlying meshes must be the same for both DOF handlers!";
  }
  std::shared_ptr<const lf::mesh::Mesh> mesh =
      dofh_Linear_FE.Mesh();                          // get the mesh
  Eigen::VectorXd zeta(dofh_Quadratic_FE.NumDofs());  // initialise empty zeta
  // safety guard: always set zero if you're not sure to set every entry later
  // on for us this shouldn't be a problem, but just to be sure
  zeta.setZero();

  for (const auto *cell : mesh->Entities(0)) {
    // check if the spaces are actually linear and quadratic
    //====================
    // Your code goes here
    //====================
    // get the global dof indices of the linear and quadratic FE spaces, note
    // that the vectors obey the LehrFEM++ numbering, which we will make use of
    // lin\_dofs will have size 3 for the 3 dofs on the nodes and
    // quad\_dofs will have size 6, the first 3 entries being the nodes and
    // the last 3 the edges
    //====================
    // Your code goes here
    // assign the coefficients of mu to the correct entries of zeta, use
    // the previous subproblem 2-9.a
    //====================

    if (dofh_Linear_FE.NumLocalDofs(*cell) != 3 ||
        dofh_Quadratic_FE.NumLocalDofs(*cell) != 6){
      throw("Error");
    }
    nonstd::span<const lf::assemble::gdof_idx_t> lin_idxs = dofh_Linear_FE.GlobalDofIndices(*cell);
    nonstd::span<const lf::assemble::gdof_idx_t> quad_idxs = dofh_Quadratic_FE.GlobalDofIndices(*cell);
    for (int i=0; i<3; i++){
      zeta(quad_idxs[i]) = mu(lin_idxs[i]);
      zeta(quad_idxs[i+3]) = 0.5*mu(lin_idxs[i])+0.5*mu(lin_idxs[(i+1)%3]);
    }
  }
  return zeta;
}
/* SAM_LISTING_END_5 */

}  // namespace LFPPDofHandling


/**
 * @file zienkiewiczzhuestimator_test.cc
 * @brief NPDE homework "ZienkiewiczZhuEstimator" code
 * @author Philipp Lindenberger
 * @date 25.03.2020
 * @copyright Developed at ETH Zurich
 */

#include <memory>
// Eigen includes
#include <gtest/gtest.h>

// Lehrfem++ includes
#include <lf/assemble/assemble.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <unsupported/Eigen/KroneckerProduct>

#include "../zienkiewiczzhuestimator.h"

namespace ZienkiewiczZhuEstimator::test {

/**
 * @brief test VectorProjectionMatrixProvider implementation
 */
TEST(ZienkiewiczZhuEstimator, VectorProjectionMatrixProvider) {
  // Load triangular mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  // Pointer to scalar FE-space
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  Eigen::Matrix2d identity = Eigen::MatrixXd::Identity(2, 2);
  auto mf_one = lf::mesh::utils::MeshFunctionConstant(1.0);
  auto mf_zero = lf::mesh::utils::MeshFunctionConstant(0.0);

  // Student solution
  ZienkiewiczZhuEstimator::VectorProjectionMatrixProvider student_elmat_builder;
  // Reference solution
  lf::uscalfe::ReactionDiffusionElementMatrixProvider<double, decltype(mf_zero),
                                                      decltype(mf_one)>
      elmat_builder_exact(fe_space_p, mf_zero, mf_one);

  // Loop over all dim-2 entities, compute error
  for (const auto tria : mesh_p->Entities(0)) {
    auto student_elem_mat = student_elmat_builder.Eval(*tria);
    auto elem_mat_scalar_exact = elmat_builder_exact.Eval(*tria);
    auto elem_mat_exact =
        Eigen::KroneckerProduct(elem_mat_scalar_exact, identity);
    double error = (student_elem_mat - elem_mat_exact).norm();
    EXPECT_LT(error, 1.0e-12);
  }
}

/**
 * @brief test GradientProjectionVectorProvider implementation
 */
TEST(ZienkiewiczZhuEstimator, GradientProjectionVectorProvider) {
  // Load triangular mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  // Pointer to scalar FE-space
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Scalar DOF-Handler
  auto &dofh = fe_space_p->LocGlobMap();

  // Solution vector 1:
  auto mu_x = [](Eigen::Vector2d x) -> double { return x[0]; };
  auto mf_mu_x = lf::mesh::utils::MeshFunctionGlobal(
      [&mu_x](Eigen::Vector2d x) -> double { return mu_x(x); });
  auto mu_x_vec = lf::fe::NodalProjection(*fe_space_p, mf_mu_x);

  // Solution vector 2:
  auto mu_y = [](Eigen::Vector2d x) -> double { return x[1]; };
  auto mf_mu_y = lf::mesh::utils::MeshFunctionGlobal(
      [&mu_y](Eigen::Vector2d x) -> double { return mu_y(x); });
  auto mu_y_vec = lf::fe::NodalProjection(*fe_space_p, mf_mu_y);

  // Retrieve unit triangle from mesh (see documentation of
  // GenerateHybrid2DTestMesh(3) on LF++)
  auto unit_tria = mesh_p->EntityByIndex(0, 1);

  // Student solutions
  ZienkiewiczZhuEstimator::GradientProjectionVectorProvider grad_vec_builder_x(
      fe_space_p, mu_x_vec);
  ZienkiewiczZhuEstimator::GradientProjectionVectorProvider grad_vec_builder_y(
      fe_space_p, mu_y_vec);

  auto unit_tria_grad_x = grad_vec_builder_x.Eval(*unit_tria);
  auto unit_tria_grad_y = grad_vec_builder_y.Eval(*unit_tria);

  for (int i = 0; i < 6; i += 2) {
    ASSERT_NEAR(unit_tria_grad_x(i), 0.25, 1.0e-8);
    ASSERT_NEAR(unit_tria_grad_x(i + 1), 0.0, 1.0e-8);
    ASSERT_NEAR(unit_tria_grad_y(i), 0.0, 1.0e-8);
    ASSERT_NEAR(unit_tria_grad_y(i + 1), 0.25, 1.0e-8);
  }
}

/**
 * @brief test computeLumpedProjection implementation
 */
TEST(ZienkiewiczZhuEstimator, computeLumpedProjection) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4);
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Obtain reference to scalar dofh
  const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};
  // Produce a dof handler for the vector-valued finite element space
  lf::assemble::UniformFEDofHandler vec_dofh(mesh_p,
                                             {{lf::base::RefEl::kPoint(), 2},
                                              {lf::base::RefEl::kSegment(), 0},
                                              {lf::base::RefEl::kTria(), 0},
                                              {lf::base::RefEl::kQuad(), 0}});

  // Solution vector 1:
  auto mu_x = [](Eigen::Vector2d x) -> double { return x[0]; };
  auto mf_mu_x = lf::mesh::utils::MeshFunctionGlobal(
      [&mu_x](Eigen::Vector2d x) -> double { return mu_x(x); });
  auto mu_x_vec = lf::fe::NodalProjection(*fe_space_p, mf_mu_x);

  // Solution vector 2:
  auto mu_y = [](Eigen::Vector2d x) -> double { return x[1]; };
  auto mf_mu_y = lf::mesh::utils::MeshFunctionGlobal(
      [&mu_y](Eigen::Vector2d x) -> double { return mu_y(x); });
  auto mu_y_vec = lf::fe::NodalProjection(*fe_space_p, mf_mu_y);

  // Student solutions
  auto sol_x = ZienkiewiczZhuEstimator::computeLumpedProjection(dofh, mu_x_vec,
                                                                vec_dofh);
  auto sol_y = ZienkiewiczZhuEstimator::computeLumpedProjection(dofh, mu_y_vec,
                                                                vec_dofh);

  for (int i = 0; i < sol_x.size(); i += 2) {
    ASSERT_NEAR(sol_x(i), 1.0, 1.0e-8);
    ASSERT_NEAR(sol_x(i + 1), 0.0, 1.0e-8);
    ASSERT_NEAR(sol_y(i), 0.0, 1.0e-8);
    ASSERT_NEAR(sol_y(i + 1), 1.0, 1.0e-8);
  }
}

/**
 * @brief test computeL2Deviation implementation
 */
TEST(ZienkiewiczZhuEstimator, computeL2Deviation) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(4);
  auto fe_space_p =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Obtain reference to scalar dofh
  const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};
  // Produce a dof handler for the vector-valued finite element space
  lf::assemble::UniformFEDofHandler vec_dofh(mesh_p,
                                             {{lf::base::RefEl::kPoint(), 2},
                                              {lf::base::RefEl::kSegment(), 0},
                                              {lf::base::RefEl::kTria(), 0},
                                              {lf::base::RefEl::kQuad(), 0}});

  auto mu = ZienkiewiczZhuEstimator::solveBVP(fe_space_p);

  // Assumes computeLumpedProjection works as expected (tested above)
  auto mu_grad =
      ZienkiewiczZhuEstimator::computeLumpedProjection(dofh, mu, vec_dofh);
  // Student solution
  double error_grad = computeL2Deviation(dofh, mu, vec_dofh, mu_grad);

  ASSERT_NEAR(error_grad, 0.0419009, 1.0e-6);
}

}  // namespace ZienkiewiczZhuEstimator::test


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/io/test_ostream.hpp>

using namespace std;

int main()
{
    using mtl::io::tout;
    using mtl::vec::parameters;

    bool with_errors= false;

    mtl::dense_vector<int>                                u(3), v, ref(3);
    mtl::dense_vector<int, parameters<mtl::row_major> >   ur(3), vr, refr(3);

    u= 1, 2, 3;
    ur= 1, 2, 3;

    v= u * u;
    tout << "u * u = " << v << endl;

    vr= ur * ur;
    tout << "ur * ur = " << vr << endl;

    ref= 1, 4, 9;
    refr= 1, 4, 9;

    if (v != ref) {
	cerr << "Wrong result in element-wise product (column vectors)" << endl;
	with_errors= true;
    }

    if (vr != refr) {
	cerr << "Wrong result in element-wise product (row vectors)" << endl;
	with_errors= true;
    }

    return with_errors ? 1 : 0;
}


/*
 * @file File for Eigen helper functions
 * Eigen_Utils.hpp
 *
 *  Created on: 25.07.2018
 *      Author: tomlucas
 */

#ifndef EIGEN_UTILS_HPP_
#define EIGEN_UTILS_HPP_

#include <Eigen/Geometry>
#include <math.h>
#include "ZaVI_Utils.hpp"
#include <ceres/ceres.h>
namespace zavi
::eigen_util {
	/**
	 * Rotates a vector with the given quaternion
	 * @param vector The vector to rotate
	 * @param quat The quaternion
	 * @return the rotatet vector
	 */
	inline ::Eigen::Vector3d rotateVector(const ::Eigen::Vector3d &vector, const ::Eigen::Quaterniond &quat) {
		::Eigen::Quaterniond vec(0, 0, 0, 0);
		vec.w() = 0;
		vec.vec() = vector;
		return (quat * vec * quat.conjugate()).vec();
	}
	/**
	 * Euler rodriguez formula
	 *
	 * Calculates a rotation matrix as if roll pitch and yaw happend simultaenously
	 * taken from C. Hertzberg 2013
	 * @param roll the roll angle
	 * @param pitch   the pitch angle
	 * @param yaw  the yaw angle
	 * @return a 3x3 rotation matrix
	 */
	template<typename T>
	::Eigen::Matrix<T, 3, 3> eulerRodriguez(const T & roll,const T & pitch,const T & yaw);

	/**
	 * Euler rodriguez formula
	 *
	 * Calculates a rotation matrix as if roll pitch and yaw happend simultaenously
	 * taken from C. Hertzberg 2013
	 * @param vec roll pitch and yaw as vector
	 * @return a 3x3 rotation matrix
	 */
	template<typename T>
	inline ::Eigen::Matrix<T, 3, 3> eulerRodriguez(const Eigen::Matrix<T, 3, 1> &vec) {
		return eulerRodriguez<T>(vec(0, 0), vec(1, 0), vec(2, 0));
	}

	/**
	 * Wraps an angle to -PI + PI
	 * @param angle the angle
	 * @return the wrapped angle
	 */
	template<typename T>
	T wrapAngle(const T & angle) {
		T temp=angle+M_PI;
		temp = fmod(temp,2*M_PI);
		if (temp < T(0.))
		temp += 2*M_PI;
		return temp - M_PI;
	}

	/**
	 * Wraps an angle to -PI + PI
	 * @param angle the angle
	 * @return the wrapped angle
	 */
	template<typename T,int size>
	ceres::Jet<T,size> wrapAngle(const ceres::Jet<T,size> & angle) {
		ceres::Jet<T,size> temp=angle;
		temp.a=wrapAngle(temp.a);
		return temp;
	}
	/**
	 * wraps the angles to -M_PI to M_PI
	 * @param matrix a eigen matrix or expression
	 * @return
	 */
	template<typename Derived>
	inline ::Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1> wrapAngles(
			const ::Eigen::MatrixBase<Derived> & matrix) {
		::Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1> temp = matrix;
		for (int i = 0; i < matrix.RowsAtCompileTime; i++) {
			temp(i) = wrapAngle(temp(i));
		}
		return temp;
	}
	/**
	 * retrieve the euler angles from an euler rodriguez rotation matrix
	 * @param rotation the rotation matrix
	 * @return euler angles as 3,1 matrix
	 */
	template<typename T>
	Eigen::Matrix<T, 3, 1> inverseEulerRodriguez(const ::Eigen::Matrix<T, 3, 3> & rotation);

	/**
	 * Generates an quaternion from euler angle
	 *
	 *taken from https://stackoverflow.com/questions/31589901/euler-to-quaternion-quaternion-to-euler-using-eigen
	 *
	 * @param roll  roll in radians
	 * @param pitch pitch in radians
	 * @param yaw  yaw in radians
	 * @return
	 */
	inline ::Eigen::Quaterniond eulerToQuaternion(double roll, double pitch, double yaw) {
		Eigen::Quaterniond q(eulerRodriguez<double>(roll, pitch, yaw));
		return q;
	}

	/**
	 * Transfers a vector to  a quaternion
	 * @param state the state vector
	 * @param startindex the start index of the euler angles
	 * @return an eigen Quaterniond
	 *
	 * state_dim dimension of the state
	 */
	template<int state_dim>
	inline ::Eigen::Quaterniond stateToQuaternion(Eigen::Matrix<double, state_dim, 1> & state, int startindex) {
		return ::Eigen::Quaterniond(state(startindex + 0), state(startindex + 1), state(startindex + 2),
				state(startindex + 3));
	}

	/**
	 * Object to call a function
	 *
	 * implements operator() to be called like a function and return a Eigen::Vector instead of a template pointer (ceres requirement)
	 *
	 * Implements an implicit type erasure so every function which needs a FuncCaller will accept snx objrvz ehivh implements the function (double * time, double * result, bool expand_noise)
	 */
	class FuncCaller {
	private:

		struct TypeErasure {
			virtual ~TypeErasure() {}
			virtual Eigen::Vector3d operator()(double time, bool expand_noise = false)const =0;

		};
		template<typename functor>
		struct FunctionWrapper: public TypeErasure {
			FunctionWrapper(const functor &function):function(function) {}

			virtual Eigen::Vector3d operator()(double time, bool expand_noise = false) const {
				double result[3];
				function(&time, result, expand_noise);
				return Eigen::Vector3d(result[0], result[1], result[2]);
			}
			functor function;
		};
	public:
		/**
		 * Calls the stored functor with parameters
		 * @param time time_point on wich the function es evaluated
		 * @param expand_noise whether to add a noisy increment
		 * @return a Eigen 3D Vector as the function result
		 */
		Eigen::Vector3d operator()(double time, bool expand_noise = false) const {
			return (*function)(time,expand_noise);
		}
		/**
		 * Create the funcCaller from an arbitrary object which implements the operator()((double * time, double * result, bool expand_noise)
		 * @param func function object
		 */
		template<typename functor>
		FuncCaller(const functor & func) :
		function(new FunctionWrapper<functor>(func)) {
		}
		/**
		 * Copy constructor
		 * @param func another FuncCaller
		 */
		FuncCaller(FuncCaller & func):
		function(func.function) {
		}

	protected:
		std::shared_ptr<TypeErasure> function;
	};

	/**
	 * First order numerical derivative
	 * @param time the time point
	 * @param function FunctionCaller   to derive
	 * @param itv the intervall for numeric derivation
	 * @return first order derivate at time
	 */

	inline Eigen::Vector3d diff1(double time,const FuncCaller & function, double itv = 1e-6) {
		return (function(time) - function(time - itv)) / itv;
	}
	/**
	 * Second order numerical derivative
	 * @param time the time point
	 * @param function  FunctionCaller   to derive
	 * @param itv the intervall for numeric derivation
	 * @return second order derivate at time
	 */
	inline Eigen::Vector3d diff2(double time,const FuncCaller & function, double itv = 1e-6) {
		return (diff1(time, function, itv) - diff1(time - itv, function, itv)) / itv;
	}
	/**
	 * Calculates the euler rotation angles of a function at a given time
	 *
	 * taken from https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
	 *
	 * @param time  the time point
	 * @param function FunctionCaller   to calculate the orientation off
	 * @param itv the intervall for derivation purposes
	 * @param roll_function a function to determine roll, if not given roll is 0
	 * @return a vector with the 3 euler angles representing the orientation
	 */
	inline Eigen::Matrix3d orientationFromFunctor(double time,const FuncCaller & function, double itv,
			FuncCaller * ref_function = NULL) {
		Eigen::Vector3d vector_orient = diff1(time, function, itv);
		vector_orient.normalize();
		Eigen::Vector3d ref_axis =
		ref_function == NULL ? Eigen::Vector3d(0, 0, 1) : Eigen::Vector3d(ref_function->operator ()(time));
		Eigen::Vector3d xaxis = ref_axis.cross(vector_orient);
		xaxis.normalize();
		Eigen::Vector3d yaxis = vector_orient.cross(xaxis);
		yaxis.normalize();
		Eigen::Matrix3d orient;
		orient.col(1) = xaxis;
		orient.col(2) = yaxis;
		orient.col(0) = vector_orient;
		assert(abs(orient.determinant()-1)< 1e-3);
		return orient;
	}
	/**
	 * Wraps a rotation delta ( x,y,z) to -M_PI  + M_PI
	 * @param delta the vector to wrap
	 * @return a delta vector with values between -M_PI and + M_PI
	 */
	template<typename T>
	Eigen::Matrix<T,3,1> wrapRotDeltaVector(const Eigen::Matrix<T,3,1> & delta) {
		T sqnorm=delta.squaredNorm();
		if(sqnorm==T(0.)) {
			return delta;
		}
		T norm=sqrt(sqnorm);
		if(norm > M_PI) {
			T wrapped_norm=zavi::eigen_util::wrapAngle(norm);
			return delta*wrapped_norm/norm;
		}
		else
		return delta;
	}
	/**
	 * make s skew symmetric from w so that Q'=S(w)*Q  where Q is a rotation matrix and w its change
	 * @param w a 3 element vector
	 * @return a skew symmetric matrix of w
	 */
	template<typename T>
	Eigen::Matrix<T,3,3> makeSkewSymmetric(const Eigen::Matrix<T,3,1> & w) {
		Eigen::Matrix<T,3,3> S=S.Zero();
		S(0,1)=-w(2,0);
		S(0,2)=w(1,0);
		S(1,2)=-w(0,0);

		S(1,0)=w(2,0);
		S(2,0)=-w(1,0);
		S(2,1)=w(0,0);
		return S;
	}
	/**
	 * Determine whether a line intersects a plane
	 *
	 * where the line is l+t*l_d
	 *
	 * and the plane is p+u*p_d1 +v*p_d2
	 *
	 * with t,u,v variables in [0,1]
	 *
	 * length of vectors determines the line length / plane width/height
	 *
	 * @param line_base line start point
	 * @param line_dir  line direction vector
	 * @param plane_base plane start point
	 * @param plane_dir1  first plane direction vector
	 * @param plane_dir2  second plane direction vector
	 * @return
	 */
	template<typename T>
	bool lineIntersectsPlane(const Eigen::Matrix<T,3,1> &line_base,const Eigen::Matrix<T,3,1> &line_dir, const Eigen::Matrix<T,3,1> &plane_base, const Eigen::Matrix<T,3,1> &plane_dir1,const Eigen::Matrix<T,3,1> &plane_dir2 ) {
		Eigen::Matrix<T,3,3> A;
		A << -line_dir,plane_dir1 , plane_dir2;
		if(A.determinant() == 0.) {
			LOG(WARNING)<< "Unhandled Special case in lineIntersectsPlane. Line may be inside plane.";
			return false;
		}
		Eigen::Matrix<T,3,1> tuv=A.inverse()*(line_base-plane_base);
		bool intersects=true;
		for(unsigned int i=0; i < 3; i++) {
			if(tuv(i) < T(0.) or tuv(i) > T(1.))
			intersects=false;
		}
		return intersects;
	}

	/**
	 * For use from other functions
	 * @param a 3d Orientation matrix
	 * @param b 3d orientation matrix
	 * @return b boxminus a
	 */
	template<typename T, typename T2>
	inline static auto boxMinusOrientation(const Eigen::Matrix<T, 3, 3> &a,
			const Eigen::Matrix<T2, 3, 3> &b) ->Eigen::Matrix<decltype(a(0,0)*b(0,0)),3,1> {
		Eigen::Matrix<decltype(a(0,0)*b(0,0)), 3, 3> product= a.inverse() * b;
		return zavi::eigen_util::inverseEulerRodriguez<decltype(a(0,0)*b(0,0))>(product);
	}

	/**
	 * For use from other functions
	 * @param a Angle Axis vector
	 * @param b Angle Axis vector
	 * @return b boxminus a
	 */
	template<typename T, typename T2>
	inline static auto boxMinusEuler(const Eigen::Matrix<T, 3, 1> &a,
			const Eigen::Matrix<T2, 3, 1> &b) ->Eigen::Matrix<decltype(a(0,0)*b(0,0)),3,1> {
		return wrapAngles(b-a);
	}

	/**
	 * For use from other functions
	 * @param a Angle Axis vector
	 * @param b Angle Axis vector
	 * @return a boxplus b
	 */
	template<typename T, typename T2>
	inline static auto boxPlusEuler(const Eigen::Matrix<T, 3, 1> &state,
			const Eigen::Matrix<T2, 3, 1> &delta) ->Eigen::Matrix<decltype(state(0,0)*delta(0,0)),3,1> {
		return wrapAngles(state+delta);
	}

	/**
	 * boxplus operator for rotations
	 * @param state  the rotation matrix
	 * @param delta  the rotation change axis angle
	 * @return state boxplus delta
	 */
	template<typename T>
	inline static Eigen::Matrix<T, 3, 3> boxPlusOrientation(const Eigen::Matrix<T, 3, 3> &state,
			const Eigen::Matrix<T, 3, 1> &delta) {
		//assert(abs(state.determinant() - T(1.)) < T(1e-2));
		Eigen::Matrix<T, 3, 3> product = state* eigen_util::eulerRodriguez(wrapRotDeltaVector(delta));
		//assert(abs(product.determinant() - T(1.)) < T(1e-6));
		return product;

	}

	inline static Eigen::Matrix3d normaliseRotation(const Eigen::Matrix3d & rotation) {
		Eigen::Matrix3d Q= rotation.householderQr().householderQ();
		for(int i=0; i < 3 ; i ++){
			if(Q(i,i)<0.){
				Q.col(i)=-(Q.col(i));
			}
		}
		return Q;
	}

	inline double norm(const double a, const double b, const double c) {
		return sqrt(pow(a, 2) + pow(b, 2) + pow(c, 2));
	}

	template <typename T, int N> inline
	ceres::Jet<T, N> norm(const ceres::Jet<T, N>& a, const ceres::Jet<T, N>& b, const ceres::Jet<T, N>& c) {
		ceres::Jet<T, N> out;

		T const temp1 =sqrt(pow(a.a, 2) + pow(b.a, 2) + pow(c.a, 2));
		T const multiplier= 1./(temp1);
		T const temp2 = temp1==T(0.)? T(1./sqrt(3.)): multiplier*(a.a);
		T const temp3 = temp1==T(0.)? T(1./sqrt(3.)): multiplier*(b.a);
		T const temp4 = temp1==T(0.)? T(1./sqrt(3.)): multiplier*(c.a);

		out.a = temp1;
		out.v = temp2 * a.v + temp3 * b.v+temp4*c.v;
		return out;
	}

}
//zavi::eigen_util

#include "Eigen_Utils.tpp"
#endif /* EIGEN_UTILS_HPP_ */


// Includes
// ========
#include <iostream>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long> > > > > graph; // new! weightmap corresponds to costs

typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it; // Iterator

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

void testcase() {

    int c, g, b, k, a;
    std::cin >> c >> g >> b >> k >> a;
    // Create graph, edge adder class and propery maps
    graph G(c + g);
    edge_adder adder(G);
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto rc_map = boost::get(boost::edge_residual_capacity, G);

    const int v_source = boost::add_vertex(G);

    int sum_elephants = 0;
    for(int i = 0; i < g; i++) {
        int x, y, d, e;
        std::cin >> x >> y >> d >> e;
        sum_elephants += e;
        adder.add_edge(x, c + i, e, d);
        adder.add_edge(c + i, y, e, 0);
    }

    // source
    adder.add_edge(v_source, k, sum_elephants, 0);

    int l = 0;
    int s_flow = 0;
    out_edge_it e, eend;
    boost::successive_shortest_path_nonnegative_weights(G, v_source, a);
    int cost = boost::find_flow_cost(G);

    for(boost::tie(e, eend) = boost::out_edges(boost::vertex(v_source,G), G); e != eend; ++e) {
        s_flow += c_map[*e] - rc_map[*e];
    }

    // this is a simple heuristic: we save the binary search
    // if cost is not the bottleneck anyway
    if(cost <= b) {
        std::cout << s_flow << std::endl;
        return;
    }

    int r = s_flow;
    while(l <= r) {
        int mid = (l + r) / 2;
        // change capacity
        const edge_desc e_s = boost::edge(v_source, k, G).first;
        const edge_desc rev_e = r_map[e_s];
        c_map[e_s] = mid;
        c_map[rev_e] = 0; // reverse edge has no capacity!
        boost::successive_shortest_path_nonnegative_weights(G, v_source, a);
        int cost = boost::find_flow_cost(G);
        out_edge_it e, eend;
        if(cost <= b) { // cost is okay, i.e. go higher
            l = mid + 1;
        } else {
            r = mid;
            if(l == r) break;
        }
    }
    std::cout << l - 1 << std::endl;
}
int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


/**
 * @date Mon May 16 21:45:27 2011 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 * @author Andre Anjos <andre.anjos@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <algorithm>
#include <vector>
#include <boost/shared_array.hpp>

#include <bob.math/eig.h>

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>

// Generalized eigenvalue decomposition of a real matrix
//   (dgeev)
extern "C" void dgeev_( const char *jobvl, const char *jobvr,
  const int *N, double *A, const int *lda, double *wr, double *wi,
  double *vl, const int *ldvl, double *vr, const int *ldvr,
  double *work, const int *lwork, int *info);

// Declaration of the external LAPACK functions
// Eigenvalue decomposition of a real symmetric matrix (dsyevd)
//   (Divide and conquer version which is supposed to be faster than dsyev)
extern "C" void dsyevd_( const char *jobz, const char *uplo, const int *N,
  double *A, const int *lda, double *W, double *work, const int *lwork,
  int *iwork, const int *liwork, int *info);

// Generalized eigenvalue decomposition of a real symmetric definite matrix
//   (dsygvd)
//   (Divide and conquer version which is supposed to be faster than dsygv)
extern "C" void dsygvd_( const int *itype, const char *jobz, const char *uplo,
  const int *N, double *A, const int *lda, double *B, const int *ldb,
  double *W, double *work, const int *lwork, const int *iwork,
  const int *liwork, int *info);

void bob::math::eig(const blitz::Array<double,2>& A,
  blitz::Array<std::complex<double>,2>& V,
  blitz::Array<std::complex<double>,1>& D)
{
  // Size variable
  const int N = A.extent(0);
  const blitz::TinyVector<int,1> shape1(N);
  const blitz::TinyVector<int,2> shape2(N,N);

  // Check
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(V);
  bob::core::array::assertZeroBase(D);

  bob::core::array::assertSameShape(A,shape2);
  bob::core::array::assertSameShape(V,shape2);
  bob::core::array::assertSameShape(D,shape1);

  bob::math::eig_(A, V, D);
}

void bob::math::eig_(const blitz::Array<double,2>& A,
  blitz::Array<std::complex<double>,2>& V,
  blitz::Array<std::complex<double>,1>& D)
{
  // Size variable
  const int N = A.extent(0);

  // Prepares to call LAPACK function
  // Initialises LAPACK variables
  const char jobvl = 'N'; // Do NOT compute left eigen-vectors
  const char jobvr = 'V'; // Compute right eigen-vectors
  int info = 0;
  const int lda = N;
  const int ldvr = N;
  double VL = 0; // notice we don't compute the left eigen-values
  const int ldvl = 1;

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_lapack = bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0));

  // temporary arrays to receive LAPACK's eigen-values and eigen-vectors
  blitz::Array<double,1> WR(D.shape()); //real part
  blitz::Array<double,1> WI(D.shape()); //imaginary part
  blitz::Array<double,2> VR(A.shape()); //right eigen-vectors

  // Calls the LAPACK function
  // A/ Queries the optimal size of the working arrays
  const int lwork_query = -1;
  double work_query;
  dgeev_( &jobvl, &jobvr, &N, A_lapack.data(), &lda, WR.data(), WI.data(),
      &VL, &ldvl, VR.data(), &ldvr, &work_query, &lwork_query, &info);

  // B/ Computes the eigenvalue decomposition
  const int lwork = static_cast<int>(work_query);
  boost::shared_array<double> work(new double[lwork]);
  dgeev_( &jobvl, &jobvr, &N, A_lapack.data(), &lda, WR.data(), WI.data(),
      &VL, &ldvl, VR.data(), &ldvr, work.get(), &lwork, &info);

  // Checks info variable
  if (info != 0) {
    throw std::runtime_error("the QR algorithm failed to compute all the eigenvalues, and no eigenvectors have been computed.");
  }

  // Copy results back from WR, WI => D
  blitz::real(D) = WR;
  blitz::imag(D) = WI;

  // Copy results back from VR => V, with two rules:
  // 1) If the j-th eigenvalue is real, then v(j) = VR(:,j), the j-th column of
  //    VR.
  // 2) If the j-th and (j+1)-st eigenvalues form a complex conjugate pair,
  // then v(j) = VR(:,j) + i*VR(:,j+1) and v(j+1) = VR(:,j) - i*VR(:,j+1).
  blitz::Range a = blitz::Range::all();
  int i=0;
  while (i<N) {
    if (std::imag(D(i)) == 0.) { //real eigen-value, consume 1
      blitz::real(V(a,i)) = VR(i,a);
      blitz::imag(V(a,i)) = 0.;
      ++i;
    }
    else { //complex eigen-value, consume 2
      blitz::real(V(a,i)) = VR(i,a);
      blitz::imag(V(a,i)) = VR(i+1,a);
      blitz::real(V(a,i+1)) = VR(i,a);
      blitz::imag(V(a,i+1)) = -VR(i+1,a);
      i += 2;
    }
  }
}

void bob::math::eigSym(const blitz::Array<double,2>& A,
  blitz::Array<double,2>& V, blitz::Array<double,1>& D)
{
  // Size variable
  const int N = A.extent(0);
  const blitz::TinyVector<int,1> shape1(N);
  const blitz::TinyVector<int,2> shape2(N,N);

  // Check
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(V);
  bob::core::array::assertZeroBase(D);

  bob::core::array::assertSameShape(A,shape2);
  bob::core::array::assertSameShape(V,shape2);
  bob::core::array::assertSameShape(D,shape1);

  bob::math::eigSym_(A, V, D);
}

void bob::math::eigSym_(const blitz::Array<double,2>& A,
  blitz::Array<double,2>& V, blitz::Array<double,1>& D)
{
  // Size variable
  const int N = A.extent(0);

  // Prepares to call LAPACK function
  // Initialises LAPACK variables
  const char jobz = 'V'; // Get both the eigenvalues and the eigenvectors
  const char uplo = 'U';
  int info = 0;
  const int lda = N;

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack;
  // Tries to use V directly
  blitz::Array<double,2> Vt = V.transpose(1,0);
  const bool V_direct_use = bob::core::array::isCZeroBaseContiguous(Vt);
  if (V_direct_use)
  {
    A_blitz_lapack.reference(Vt);
    // Ugly fix for non-const transpose
    A_blitz_lapack = const_cast<blitz::Array<double,2>&>(A).transpose(1,0);
  }
  else
    // Ugly fix for non-const transpose
    A_blitz_lapack.reference(
      bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double *A_lapack = A_blitz_lapack.data();
  blitz::Array<double,1> D_blitz_lapack;
  const bool D_direct_use = bob::core::array::isCZeroBaseContiguous(D);
  if (D_direct_use)
    D_blitz_lapack.reference(D);
  else
    D_blitz_lapack.resize(D.shape());
  double *D_lapack = D_blitz_lapack.data();

  // Calls the LAPACK function
  // A/ Queries the optimal size of the working arrays
  const int lwork_query = -1;
  double work_query;
  const int liwork_query = -1;
  int iwork_query;
  dsyevd_( &jobz, &uplo, &N, A_lapack, &lda, D_lapack, &work_query,
    &lwork_query, &iwork_query, &liwork_query, &info);
  // B/ Computes the eigenvalue decomposition
  const int lwork = static_cast<int>(work_query);
  boost::shared_array<double> work(new double[lwork]);
  const int liwork = static_cast<int>(iwork_query);
  boost::shared_array<int> iwork(new int[liwork]);
  dsyevd_( &jobz, &uplo, &N, A_lapack, &lda, D_lapack, work.get(), &lwork,
    iwork.get(), &liwork, &info);

  // Checks info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK function 'dsyevd' returned a non-zero value.");

  // Copy singular vectors back to V if required
  if (!V_direct_use)
    Vt = A_blitz_lapack;

  // Copy result back to sigma if required
  if (!D_direct_use)
    D = D_blitz_lapack;
}

void bob::math::eigSym(const blitz::Array<double,2>& A, const blitz::Array<double,2>& B,
  blitz::Array<double,2>& V, blitz::Array<double,1>& D)
{
  // Size variable
  const int N = A.extent(0);
  const blitz::TinyVector<int,1> shape1(N);
  const blitz::TinyVector<int,2> shape2(N,N);
  bob::core::array::assertZeroBase(A);
  bob::core::array::assertZeroBase(B);
  bob::core::array::assertZeroBase(V);
  bob::core::array::assertZeroBase(D);

  bob::core::array::assertSameShape(A,shape2);
  bob::core::array::assertSameShape(B,shape2);
  bob::core::array::assertSameShape(V,shape2);
  bob::core::array::assertSameShape(D,shape1);

  bob::math::eigSym_(A, B, V, D);
}

void bob::math::eigSym_(const blitz::Array<double,2>& A, const blitz::Array<double,2>& B,
  blitz::Array<double,2>& V, blitz::Array<double,1>& D)
{
  // Size variable
  const int N = A.extent(0);

  // Prepares to call LAPACK function
  // Initialises LAPACK variables
  const int itype = 1;
  const char jobz = 'V'; // Get both the eigenvalues and the eigenvectors
  const char uplo = 'U';
  int info = 0;
  const int lda = N;
  const int ldb = N;

  // Initialises LAPACK arrays
  blitz::Array<double,2> A_blitz_lapack;
  // Tries to use V directly
  blitz::Array<double,2> Vt = V.transpose(1,0);
  const bool V_direct_use = bob::core::array::isCZeroBaseContiguous(Vt);
  if (V_direct_use)
  {
    A_blitz_lapack.reference(Vt);
    // Ugly fix for non-const transpose
    A_blitz_lapack = const_cast<blitz::Array<double,2>&>(A).transpose(1,0);
  }
  else
    // Ugly fix for non-const transpose
    A_blitz_lapack.reference(
      bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(A).transpose(1,0)));
  double *A_lapack = A_blitz_lapack.data();
  // Ugly fix for non-const transpose
  blitz::Array<double,2> B_blitz_lapack(
    bob::core::array::ccopy(const_cast<blitz::Array<double,2>&>(B).transpose(1,0)));
  double *B_lapack = B_blitz_lapack.data();
  blitz::Array<double,1> D_blitz_lapack;
  const bool D_direct_use = bob::core::array::isCZeroBaseContiguous(D);
  if (D_direct_use)
    D_blitz_lapack.reference(D);
  else
    D_blitz_lapack.resize(D.shape());
  double *D_lapack = D_blitz_lapack.data();

  // Calls the LAPACK function
  // A/ Queries the optimal size of the working arrays
  const int lwork_query = -1;
  double work_query;
  const int liwork_query = -1;
  int iwork_query;
  dsygvd_( &itype, &jobz, &uplo, &N, A_lapack, &lda, B_lapack, &ldb, D_lapack,
    &work_query, &lwork_query, &iwork_query, &liwork_query, &info);
  // B/ Computes the generalized eigenvalue decomposition
  const int lwork = static_cast<int>(work_query);
  boost::shared_array<double> work(new double[lwork]);
  const int liwork = static_cast<int>(iwork_query);
  boost::shared_array<int> iwork(new int[liwork]);
  dsygvd_( &itype, &jobz, &uplo, &N, A_lapack, &lda, B_lapack, &ldb, D_lapack,
    work.get(), &lwork, iwork.get(), &liwork, &info);

  // Checks info variable
  if (info != 0)
    throw std::runtime_error("The LAPACK function 'dsygvd' returned a non-zero value. This might be caused by a non-positive definite B matrix.");

  // Copy singular vectors back to V if required
  if (!V_direct_use)
    V = A_blitz_lapack.transpose(1,0);

  // Copy result back to sigma if required
  if (!D_direct_use)
    D = D_blitz_lapack;
}


#include "./include/rsa.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <string>

int main(int argc, char const *argv[])
{
    RSA::key_generator key_gen(512);
    auto k1 = key_gen.get_public_key_1();
    auto k2 = key_gen.get_public_key_2();
    auto k3 = key_gen.get_private_key();

    std::cout << "Chave pública 1:\t" << k1 << std::endl;
    std::cout << "Chave pública 2:\t" << k2 << std::endl;
    std::cout << "Chave privada:\t\t" << k3 << std::endl;

    RSA::encoder manager;
    std::string file("messages/text.txt");
    std::string file1("messages/encoded.txt");
    std::string file2("messages/decoded.txt");

    manager.encode(file, file1, k1, k2);
    manager.decode(file1, file2, k1, k3);

    return 0;
}


#ifndef TEST_UNIT_TORSTEN_PK_FRIBERG_KARLSSON_MODEL_TEST_FIXTURE
#define TEST_UNIT_TORSTEN_PK_FRIBERG_KARLSSON_MODEL_TEST_FIXTURE

#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <stan/math/rev/fun/pow.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

struct FribergKarlsson {
   // parms contains both the PK and the PD parameters.
   // x contains both the PK and the PD states.
  template <typename T0, typename T1, typename T2, typename T3>
  inline
  Eigen::Matrix<typename stan::return_type_t<T0, T1, T2, T3>, -1, 1>
  operator()(const T0& t,
             const Eigen::Matrix<T1, -1, 1>& x,
             std::ostream* pstream__,
             const std::vector<T2>& parms,
             const std::vector<T3>& x_r,
             const std::vector<int>& x_i) const {
    using Eigen::Matrix;
    using Eigen::Dynamic;
    using scalar = typename stan::return_type_t<T0, T1, T2, T3>;

    // PK variables
    T2
      CL = parms[0],
      Q = parms[1],
      VC = parms[2],
      VP = parms[3],
      ka = parms[4],
      k10 = CL / VC,
      k12 = Q / VC,
      k21 = Q / VP;

    // PD variables
    T2
      MTT = parms[5],
      circ0 = parms[6],
      gamma = parms[7],
      alpha = parms[8],
      ktr = 4 / MTT;
    typename stan::return_type_t<T1, T2>
      prol = x[3] + circ0,
      transit1 = x[4] + circ0,
      transit2 = x[5] + circ0,
      transit3 = x[6] + circ0,
      circ = stan::math::fmax(stan::math::machine_precision(), x[7] + circ0);

    Eigen::Matrix<scalar, -1, 1> dxdt(8);
    dxdt[0] = -ka * x[0];
    dxdt[1] = ka * x[0] - (k10 + k12) * x[1] + k21 * x[2];
    dxdt[2] = k12 * x[1] - k21 * x[2];

    scalar conc = x[1] / VC;
    scalar Edrug = alpha * conc;

    dxdt[3] = ktr * prol * (((1 - Edrug) * pow((circ0 / circ), gamma)) - 1);
    dxdt[4] = ktr * (prol - transit1);
    dxdt[5] = ktr * (transit1 - transit2);
    dxdt[6] = ktr * (transit2 - transit3);
    dxdt[7] = ktr * (transit3 - circ);

    return dxdt;
  }
};

struct FribergKarlssonTest : public testing::Test {
  const FribergKarlsson f;
  int nCmt;
  int nt;
  std::vector<std::vector<double> > theta;
  std::vector<std::vector<double> > biovar;
  std::vector<std::vector<double> > tlag;
  std::vector<double> time;
  std::vector<double> amt;
  std::vector<double> rate;
  std::vector<int> cmt;
  std::vector<int> evid;
  std::vector<double> ii;
  std::vector<int> addl;
  std::vector<int> ss;

  void SetUp() {
    // make sure memory's clean before starting each test
    stan::math::recover_memory();
  }

  FribergKarlssonTest() :
    f(),
    nCmt(8),
    nt(10),
    // CL , Q , Vc , Vp , ka , MTT , Circ0 , alpha , gamma
    theta(1, {10, 15, 35, 105, 2.0, 125, 5, 0.17, 3e-4}),
    biovar(1, {1, 1, 1, 1, 1, 1, 1, 1}),
    tlag(1, {0, 0, 0, 0, 0, 0, 0, 0}),
    time(nt),
    amt(nt, 0),
    rate(nt, 0),
    cmt(nt, 2),
    evid(nt, 0),
    ii(nt, 0),
    addl(nt, 0),
    ss(nt, 0)
  {
    time[0] = 0.0;
    for(int i = 1; i < nt; i++) time[i] = i * 1.25;

    amt[0] = 80 * 1000.0;
    cmt[0] = 1;
    evid[0] = 1;
    ii[0] = 12;

    SetUp();
  }

  void resize(int n) {
    nt = n;
    time.resize(nt);
    amt .resize(nt);
    rate.resize(nt);
    cmt .resize(nt);
    evid.resize(nt);
    ii  .resize(nt);
    addl.resize(nt);
    ss  .resize(nt);
  }
};

#endif


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * 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.
 */

///
/// \file aak_reduction.hpp
///
/// Sparse approximation of exponential sum using modified Prony method.
///
#ifndef MXPFIT_MODIFIED_PRONY_REDUCTION_HPP
#define MXPFIT_MODIFIED_PRONY_REDUCTION_HPP

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>

#include <mxpfit/exponential_sum.hpp>

namespace mxpfit
{
///
/// ### ModifiedPronyReduction
///
/// \brief Find a truncated exponential sum function with smaller number of
///        terms by the modified balanced truncation method.
///
/// \tparam T  Scalar type of exponential sum function.
///
/// Let us consider an exponential sum function, in which the weights and
/// exponetns are strictly positive, i.e.,
///
/// \f[
///   f(t)=\sum_{j=1}^{n} w_{j}^{} e^{-a_{j}^{} t}, \quad
///   (a_{j} > 0, \, w_{j} > 0).
/// \f]
///
/// This class calculates truncated exponential \f$\hat{f}(t)\f$ sum such that
///
/// \f[
///   \hat{f}(t)=\sum_{j=1}^{k} \hat{w}_{j}^{}e^{-\hat{a}_{j}^{} t}, \quad
///   \left| f(t)-\hat{f}(t) \right| < \epsilon, \, (k < n)
/// \f]
///
/// where \f$\epsilon > 0\f$ is the prescribed accuracy. The weights
/// \f$\hat{w}_{j}\f$ and exponents \f$\hat{w}_{j}\f$ in the trucated sum are
/// all positive.
///
/// The modified Prony method proposed by Beylkin and Monzon are adopted. We
/// refer to the literature listed below for the detial about the method.
///
/// #### References
///
/// 1. G. Beylkin and L. Monz\'{o}n, "Approximation by exponential sums
///    revisited", Appl. Comput. Harmon. Anal. 28 (2010) 131-149.
///    [DOI: https://doi.org/10.1016/j.acha.2009.08.011]
/// 2. W. McLean, "Exponential sum approximations for \f$t^{-\beta}\f$",
///    arXiv:1606.00123 [math]
///
template <typename T>
class ModifiedPronyReduction
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ResultType = ExponentialSum<Scalar, Scalar>;

    ///
    /// Compute truncated exponential sum \f$ \hat{f}(t) \f$
    ///
    /// \tparam DerivedF type of exponential sum inheriting ExponentialSumBase
    ///
    /// \param[in] orig original exponential sum function, \f$ f(t) \f$
    /// \param[in] n_target Index smaller than `orig.size()`, which denotes
    ///            number of terms to be target for reduction.
    /// \param[in] threshold  prescribed accuracy \f$0 < \epsilon \ll 1\f$
    ///
    /// \pre The weights \f$w_{j}\f$ and exponents \f${a_{j}}\f$ are strictly
    /// positive and \f$a_{j}\f$ must be sorted in ascending order.
    ///
    /// \remark The exponents \f$\hat{a}_{j}\f$ of truncated sum are obtained as
    /// a root of Prony polynomials, which might not be real and positive.
    /// Internaly, obtaind roots of polynomial is casted to be a real number,
    /// which might introduce significant errors in the approximation.
    ///
    /// \return An instance of ExponentialSum represents \f$\hat{f}(t)\f$
    ///
    template <typename DerivedF>
    ResultType compute(const ExponentialSumBase<DerivedF>& orig, Index n_target,
                       RealScalar threshold);
};

template <typename T>
template <typename DerivedF>
typename ModifiedPronyReduction<T>::ResultType
ModifiedPronyReduction<T>::compute(const ExponentialSumBase<DerivedF>& orig,
                                   Index n_target, RealScalar eps)
{
    using Eigen::numext::abs;
    using Eigen::numext::real;
    assert(Index(0) <= n_target && n_target <= orig.size());
    assert(eps > RealScalar());

    if (n_target == Index())
    {
        ResultType ret(orig);
        return ret; // quick return
    }

    //
    // Compute a sequence
    //
    // \f[
    //   h_{j} = \sum_{k=1}^{n} w_{k} a_{k}^{j}
    // \f]
    //
    VectorType h(2 * n_target);
    const auto w_target = orig.weights().head(n_target);
    const auto a_target = orig.exponents().head(n_target);
    VectorType a_pow(a_target);

    h(0) = w_target.sum();
    h(1) = -(w_target * a_pow.array()).sum();

    Index m        = 1;
    auto factorial = RealScalar(1);
    for (; m < n_target; ++m)
    {
        a_pow.array() *= a_target;
        h(2 * m + 0) = (w_target * a_pow.array()).sum();
        a_pow.array() *= a_target;
        h(2 * m + 1) = -(w_target * a_pow.array()).sum();
        factorial *= RealScalar(2 * m * (2 * m + 1));

        if (abs(h(2 * m + 1)) / factorial < eps)
        {
            // Taylor expansion converges with the tolerance eps.
            ++m;
            break;
        }
    }

    if (m == n_target)
    {
        // no further reduction
        ResultType ret(orig);
        return ret;
    }

    //
    // Construct a Hankel matrix from the sequence h, and solve the linear
    // equation, H q = b, with b = -h(m:2m-1).
    //
    MatrixType H(m, m);
    for (Index i = 0; i < m; ++i)
    {
        H.col(i) = h.segment(i, m);
    }
    VectorType q(H.colPivHouseholderQr().solve(-h.segment(m, m)));

    //
    // Find the roots of the Prony polynomial,
    //
    // q(z) = \sum_{k=0}^{m-1} q_k z^{k}.
    //
    // The roots of q(z) can be obtained as the eigenvalues of the companion
    // matrix,
    //
    //     (0  0  ...  0 -p[0]  )
    //     (1  0  ...  0 -p[1]  )
    // C = (0  1  ...  0 -p[2]  )
    //     (.. .. ...  .. ..    )
    //     (0  0  ...  1 -p[m-1])
    //

    MatrixType companion(MatrixType::Zero(m, m));
    companion.diagonal(-1).setOnes();
    companion.col(m - 1) = -q;
    VectorType gamma(companion.eigenvalues().real());

    // --- Update exponents & weights
    const Index keep = orig.size() - n_target;
    ResultType ret(keep + m);
    ret.exponents().head(m)    = -gamma;
    ret.exponents().tail(keep) = orig.exponents().tail(keep);

    //
    // Construct Vandermonde matrix from Prony roots
    //
    MatrixType V(2 * m, m);
    for (Index i = 0; i < m; ++i)
    {
        const RealScalar z = gamma(i);
        V(0, i)            = RealScalar(1);
        for (Index j = 1; j < V.rows(); ++j)
        {
            V(j, i) = V(j - 1, i) * z; // z[i]**j
        }
    }

    //
    // Solve overdetermined Vandermonde system,
    //
    // V(0:2m-1,0:m-1) w(0:m-1) = h(0:2m-1)
    //
    // by the least square method.
    //
    ret.weights().head(m)    = V.colPivHouseholderQr().solve(h.head(2 * m));
    ret.weights().tail(keep) = orig.weights().tail(keep);

    return ret;
}

} // namespace mxpfit

#endif /* MXPFIT_MODIFIED_PRONY_REDUCTION_HPP */


#pragma once

#include <cmath>

#include <Eigen/Dense>

#define D2R(x) (x * (M_PI/180))
#define R2D(x) (x * (180/M_PI))

class Geodesy{
public:
	/**WGS84 ellipsoid semi-major axis*/
	static constexpr double a = 6378137.0;

	/**WGS84 ellipsoid first eccentricity squared*/
	static constexpr double e2 = 0.081819190842622 * 0.081819190842622;

	static void getPositionECEF(Eigen::Vector3d & positionECEF, double longitude, double latitude,double ellipsoidalHeight) {
		double clat = cos(latitude);
		double slat = sin(latitude);
		double clon = cos(longitude);
		double slon = sin(longitude);

		double N = a / (sqrt(1 - e2 * slat * slat));
		double xTRF = (N + ellipsoidalHeight) * clat * clon;
		double yTRF = (N + ellipsoidalHeight) * clat * slon;
		double zTRF = (N * (1 - e2) + ellipsoidalHeight) * slat;

		positionECEF << xTRF, yTRF, zTRF;
	}
};


/*
 * Sha1.cpp
 *
 *  Created on: Nov 25, 2016
 *      Author: victor
 */

#include "../../algorithms/sha/Sha1.h"

#include <boost/assert.hpp>
#include <string>
#include <iostream>

#include "../../binary/BinaryString.h"
#include "../../binary/BinaryWord.h"
#include "../../binary/Endianness.h"
#include "../../binary/tools/BinaryStringUtils.h"
#include "../../binary/tools/Utils.h"
using namespace std;

std::string algorithms::Sha1::preprocess_message(const std::string& initial_message) {
	std::string tmp_message;

	// Check assumption: sizeof(char) = 8
	BOOST_ASSERT_MSG(BYTE_TO_BIT_SIZE(sizeof(char)) == 8,
			"This function expects a char size of 1 byte.");

	// -------------------------
	// Message preprocessing
	// -------------------------
	std::string  message = initial_message;
	// Append the bit '1' to the message e.g. by adding 0x80 if message length is a multiple of 8 bits.
	message += 0x80;

	// Append 0 ≤ k < 512 bits '0', such that the resulting message length in bits
	// is congruent to −64 ≡ 448 (mod 512)
	message = algorithms::utils::pad_with_character(message, 0x00,
			BIT_TO_BYTE_SIZE(ChunkSizes::CHUNK_BIT_SIZE),
			BIT_TO_BYTE_SIZE(ChunkSizes::LAST_CHUNK_BIT_SIZE));

	// Append ml, the original message length, as a 64-bit big-endian integer. Thus, the total
	// length is a multiple of 512 bits.
	message += algorithms::utils::length_to_string_64b(BYTE_TO_BIT_SIZE(initial_message.size()));

	return message;
}

void algorithms::Sha1::word_array_init(unsigned int chunk_id,
		BinaryString& bin_message, vector<std::uint32_t>& words) {

	bin_message.get_words_from_chunk(chunk_id, words);
	words.resize(80);

	// Extend the sixteen 32-bit words into eighty 32-bit words:
	for (unsigned int i = 16; i < 80; ++i) {
		words[i] = algorithms::utils::circular_left_shift<1>(
				words[i - 3] ^ words[i - 8] ^ words[i - 14] ^ words[i - 16]);
	}
}

void algorithms::Sha1::main_loop(std::uint32_t& h0, std::uint32_t& h1,
		std::uint32_t& h2, std::uint32_t& h3, std::uint32_t& h4,
		const vector<std::uint32_t>& words) {

	//TODO: MUST addition be % 2^32?

	std::uint32_t a, b, c, d, e, f, k, temp;

	// Initialize hash value for this chunk:
	a = h0;
	b = h1;
	c = h2;
	d = h3;
	e = h4;

	// Main loop:
	for (unsigned int i = 0; i < 80; ++i){
		if (i <= 19){
			f = (b & c) | ((~b) & d);
			k = 0x5A827999;
		}
		else{
			if (i >= 20 && i <= 39){
				f = b ^ c ^ d;
				k = 0x6ED9EBA1;
			}
			else{
				if (i>= 40 && i <= 59){
					f = (b & c) | (b & d) | (c & d);
					k = 0x8F1BBCDC;
				}
				else{
					if (i>= 60 && i <= 79){
						f = b ^ c ^ d;
						k = 0xCA62C1D6;
					}
				}
			}
		}
		temp = algorithms::utils::circular_left_shift<5>(a) + f + e + k + words[i];
		e = d;
		d = c;
		c = algorithms::utils::circular_left_shift<30>(b);
		b = a;
		a = temp;
	}
	// From https://www.ipa.go.jp/security/rfc/RFC3174EN.html
		// Better version for parallelization
		/*unsigned int t;
		for (t = 0; t < 20; t++) {
			temp = SHA1CircularShift(5, a) + ((b & c) | ((~b) & d)) + e + words[t]
					+ 0x5A827999;
			e = d;
			d = c;
			c = SHA1CircularShift(30, b);
			b = a;
			a = temp;
		}
		for (t = 20; t < 40; t++) {
			temp = SHA1CircularShift(5,a) + (b ^ c ^ d) + e + words[t] + 0x6ED9EBA1;
			e = d;
			d = c;
			c = SHA1CircularShift(30, b);
			b = a;
			a = temp;
		}
		for (t = 40; t < 60; t++) {
			temp = SHA1CircularShift(5, a) + ((b & c) | (b & d) | (c & d)) + e
					+ words[t] + 0x8F1BBCDC;
			e = d;
			d = c;
			c = SHA1CircularShift(30, b);
			b = a;
			a = temp;
		}
		for (t = 60; t < 80; t++) {
			temp = SHA1CircularShift(5,a) + (b ^ c ^ d) + e + words[t] + 0xCA62C1D6;
			e = d;
			d = c;
			c = SHA1CircularShift(30, b);
			b = a;
			a = temp;
		}*/

	// Add this chunk's hash to result so far:
	h0 += a;
	h1 += b;
	h2 += c;
	h3 += d;
	h4 += e;
}

std::string algorithms::Sha1::calculate_hash(const std::string& initial_message) {

	std::uint32_t h0 = InitialHashValues::H0;
	std::uint32_t h1 = InitialHashValues::H1;
	std::uint32_t h2 = InitialHashValues::H2;
	std::uint32_t h3 = InitialHashValues::H3;
	std::uint32_t h4 = InitialHashValues::H4;

	std::string message = preprocess_message(initial_message);

	// Process the message in successive 512-bit chunks:
	// break message into 512-bit chunks
	BinaryString bin_message(message);
	unsigned int number_of_chunks = bin_message.get_num_512b_chunks();

	vector<std::uint32_t> words;
	for(unsigned int chunk_id = 0; chunk_id < number_of_chunks; ++chunk_id){

		word_array_init(chunk_id, bin_message, words);

		main_loop(h0, h1, h2, h3, h4, words);

	}

	// Produce the final hash value (big-endian) as a 160 bit number:
	string hash = BinaryWord(h0, Endianness::BIG).to_string() +
			BinaryWord(h1, Endianness::BIG).to_string() +
			BinaryWord(h2, Endianness::BIG).to_string() +
			BinaryWord(h3, Endianness::BIG).to_string() +
			BinaryWord(h4, Endianness::BIG).to_string();

	return hash;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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)

#include <sstream>

#include <boost/mpl/if.hpp>
#include <geometry_test_common.hpp>

#include <boost/geometry/geometry.hpp>

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

#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/strategies/strategies.hpp>

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

template <typename P>
void test_distance_result()
{
    typedef typename bg::default_distance_result<P, P>::type distance_type;

    P p1 = bg::make<P>(0, 0);
    P p2 = bg::make<P>(3, 0);
    P p3 = bg::make<P>(0, 4);

    distance_type dr12 = bg::comparable_distance(p1, p2);
    distance_type dr13 = bg::comparable_distance(p1, p3);
    distance_type dr23 = bg::comparable_distance(p2, p3);

    BOOST_CHECK_CLOSE(dr12, 9.000, 0.001);
    BOOST_CHECK_CLOSE(dr13, 16.000, 0.001);
    BOOST_CHECK_CLOSE(dr23, 25.000, 0.001);

}

template <typename P>
void test_distance_point()
{
    P p1;
    bg::set<0>(p1, 1);
    bg::set<1>(p1, 1);

    P p2;
    bg::set<0>(p2, 2);
    bg::set<1>(p2, 2);

    typename bg::coordinate_type<P>::type d = bg::comparable_distance(p1, p2);
    BOOST_CHECK_CLOSE(d, 2.0, 0.001);
}

template <typename P>
void test_distance_segment()
{
    typedef typename bg::coordinate_type<P>::type coordinate_type;

    P s1 = bg::make<P>(2, 2);
    P s2 = bg::make<P>(3, 3);

    // Check points left, right, projected-left, projected-right, on segment
    P p1 = bg::make<P>(0, 0);
    P p2 = bg::make<P>(4, 4);
    P p3 = bg::make<P>(2.4, 2.6);
    P p4 = bg::make<P>(2.6, 2.4);
    P p5 = bg::make<P>(2.5, 2.5);

    bg::model::referring_segment<P const> const seg(s1, s2);

    coordinate_type d1 = bg::comparable_distance(p1, seg); BOOST_CHECK_CLOSE(d1, 8.0, 0.001);
    coordinate_type d2 = bg::comparable_distance(p2, seg); BOOST_CHECK_CLOSE(d2, 2.0, 0.001);
    coordinate_type d3 = bg::comparable_distance(p3, seg); BOOST_CHECK_CLOSE(d3, 0.02, 0.001);
    coordinate_type d4 = bg::comparable_distance(p4, seg); BOOST_CHECK_CLOSE(d4, 0.02, 0.001);
    coordinate_type d5 = bg::comparable_distance(p5, seg); BOOST_CHECK_CLOSE(d5, 0.0, 0.001);

    // Reverse case
    coordinate_type dr1 = bg::comparable_distance(seg, p1); BOOST_CHECK_CLOSE(dr1, d1, 0.001);
    coordinate_type dr2 = bg::comparable_distance(seg, p2); BOOST_CHECK_CLOSE(dr2, d2, 0.001);
}

template <typename P>
void test_distance_linestring()
{
    bg::model::linestring<P> points;
    points.push_back(bg::make<P>(1, 1));
    points.push_back(bg::make<P>(3, 3));

    P p = bg::make<P>(2, 1);

    typename bg::coordinate_type<P>::type d = bg::comparable_distance(p, points);
    BOOST_CHECK_CLOSE(d, 0.5, 0.001);

    p = bg::make<P>(5, 5);
    d = bg::comparable_distance(p, points);
    BOOST_CHECK_CLOSE(d, 8.0, 0.001);

    bg::model::linestring<P> line;
    line.push_back(bg::make<P>(1,1));
    line.push_back(bg::make<P>(2,2));
    line.push_back(bg::make<P>(3,3));

    p = bg::make<P>(5, 5);

    d = bg::comparable_distance(p, line);
    BOOST_CHECK_CLOSE(d, 8.0, 0.001);

    // Reverse case
    d = bg::comparable_distance(line, p);
    BOOST_CHECK_CLOSE(d, 8.0, 0.001);
}

template <typename P>
void test_all()
{
    test_distance_result<P>();
    test_distance_point<P>();
    test_distance_segment<P>();
    test_distance_linestring<P>();
}

int test_main(int, char* [])
{
    //test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif
    return 0;
}


///
// ALGOLAB BGL Tutorial 3
// Flow example demonstrating
// - breadth first search (BFS) on the residual graph

// Compile and run with one of the following:
// g++ -std=c++11 -O2 bgl_residual_bfs.cpp -o bgl_residual_bfs ./bgl_residual_bfs
// g++ -std=c++11 -O2 -I path/to/boost_1_58_0 bgl_residual_bfs.cpp -o bgl_residual_bfs; ./bgl_residual_bfs

// Includes
// ========
// STL includes
#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// BGL graph definitions
// =====================
// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long> > > > > graph; // new! weightmap corresponds to costs

typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it; // Iterator

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

// Main
void testcase() {
  // build graph
  int n;
  std::cin >> n;
  graph G(n);
  edge_adder adder(G);
  // auto rc_map = boost::get(boost::edge_residual_capacity, G);
  const int v_source = boost::add_vertex(G);
  const int v_sink = boost::add_vertex(G);

  std::vector<int> a_and_c(2*n);
  for(int i = 0; i < n; i++) {
    int a, c;
    std::cin >> a >> c;
    a_and_c[2 * i] = a;
    a_and_c[2 * i + 1] = c;
    // adder.add_edge(v_source, i, a, c);
  }

  int sum_students = 0;
  int max_price = 0;
  std::vector<int> s_and_p(2*n);
  for(int i = 0; i < n; i++) {
    int s, p;
    std::cin >> s >> p;
    sum_students += s;
    s_and_p[2 * i] = s;
    s_and_p[2 * i + 1] = p;
    max_price = std::max(max_price, p);
  }

  for(int i = 0; i < n; i++) {
    int a = a_and_c[2 * i];
    int c = a_and_c[2 * i + 1];
    adder.add_edge(v_source, i, a, c);
  }

  for(int i = 0; i < n; i++) {
    int s = s_and_p[2 * i];
    int p = s_and_p[2 * i + 1];
    adder.add_edge(i, v_sink, s, - p + max_price);
  }

  for(int i = 0; i < n - 1; i++) {
    int v, e;
    std::cin >> v >> e;
    adder.add_edge(i, i + 1, v, e);
  }

  // int flow = boost::push_relabel_max_flow(G, v_source, v_sink);
  // boost::cycle_canceling(G);
  // int cost = boost::find_flow_cost(G);

  boost::successive_shortest_path_nonnegative_weights(G, v_source, v_sink);
  int cost = boost::find_flow_cost(G);
  // Iterate over all edges leaving the source to sum up the flow values.
  int flow = 0;
  out_edge_it e, eend;
  auto c_map = boost::get(boost::edge_capacity, G);
  auto rc_map = boost::get(boost::edge_residual_capacity, G);
  for(boost::tie(e, eend) = boost::out_edges(boost::vertex(v_source,G), G); e != eend; ++e) {
      // std::cout << "edge from " << boost::source(*e, G) << " to " << boost::target(*e, G)
      //     << " with capacity " << c_map[*e] << " and residual capacity " << rc_map[*e] << "\n";
      flow += c_map[*e] - rc_map[*e];
  }
  if(flow == sum_students) {
    std::cout << "possible ";
  } else {
    std::cout << "impossible ";
  }
  std::cerr << flow * max_price << std::endl;
  std::cout << flow << " " << -(cost - (flow * max_price)) << "\n";
  // std::cerr << std::endl;
  // // Retrieve the capacity map and reverse capacity map
  // const auto c_map = boost::get(boost::edge_capacity, G);
  // const auto rc_map = boost::get(boost::edge_residual_capacity, G);

  // // Iterate over all the edges to print the flow along them
  // auto edge_iters = boost::edges(G);
  // for (auto edge_it = edge_iters.first; edge_it != edge_iters.second; ++edge_it) {
  //   const edge_desc edge = *edge_it;
  //   const long flow_through_edge = c_map[edge] - rc_map[edge];
  //   std::cerr << "edge from " << boost::source(edge, G) << " to " << boost::target(edge, G)
  //             << " runs " << flow_through_edge
  //             << " units of flow (negative for reverse direction). \n";
  // }
}

int main() {
  std::ios_base::sync_with_stdio(false);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}


#include <iostream>
#include <array>
#include <string>
#include <cstring>
#include <fstream>
#include <exception>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include "port_authority.hpp"

using namespace std;
using namespace pauth;

#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/token_functions.hpp>

using namespace boost;
using namespace boost::program_options;

#include "mpi.h"

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

  int taskid, numtasks;
  MPI_Init(nullptr, nullptr);
  MPI_Comm_size(MPI_COMM_WORLD, &numtasks);
  MPI_Comm_rank(MPI_COMM_WORLD, &taskid);

  options_description desc("\nMetropolis simulation of a harmonic oscillator."
                           "\n\nAllowed arguments");

  double delta_max, x0, dx, sk;
  unsigned long nsteps;

  desc.add_options()
    ("help,h", "Produce this help message.")
    ("x0,x", value<double>(&x0)->default_value(0.0), "Initial position.")
    ("spring-const,k", value<double>(&sk)->default_value(1.0), "Spring constant.")
    ("beta,b", value<double>()->default_value(1.0), "1 / kT")
    ("delta,d", value<double>(&delta_max)->default_value(2.0), "Maximum step size.")
    ("num-steps,n", value<size_t>(&nsteps)->default_value(100000), "Number of steps.")
    ("dx,e", value<double>(&dx)->default_value(0.1), "Threshold size for "
      "approximating <dirac_delta(x - x0)> which is used to approximate Z.")
    ("plot-histogram,p", "Plot histogram.");

  variables_map vm;
  try {
    store(command_line_parser(argc, argv).options(desc).run(), vm);
    notify(vm);
  } catch(std::exception &e) {
    cout << endl << e.what() << endl;
    cout << desc << endl;
  }

  if (vm.count("help")) {
    if(taskid == 0) {
      cout << "--help specified" << endl;
      cout << desc << endl;
    }
    MPI_Finalize();
    return EXIT_SUCCESS;
  }

  const double T = 1.0;
  const double kB = 1.0 / vm["beta"].as<double>();

  const molecular_id id = molecular_id::Test1;
  const size_t N = 1;
  const size_t D = 1;
  const double L = 1.0;
  const metric m = euclidean;
  const bc boundary = no_bc;
  const_k_spring_potential pot(sk);
  vector<double> xs;

  metropolis sim(id, N, D, L, continuous_trial_move(delta_max),
                 &pot, T, kB, m, boundary, metropolis_acc,
                 hardware_entropy_seed_gen, true);
  sim.positions()(0, 0) = x0;
  const double u0 = accessors::U(sim);

  if (vm.count("plot-histogram")) {
    sim.add_callback([&](const metropolis &sim) -> void {
      xs.push_back(sim.positions()(0, 0));
    });
  }

  metropolis_suite msuite(sim, 0, 1, info_lvl_flag::VERBOSE);

  msuite.add_variable_to_average("x", [](const metropolis &sim) {
    return sim.positions()(0, 0);
  });
  msuite.add_variable_to_average("x^2", [](const metropolis &sim) {
    return sim.positions()(0, 0) * sim.positions()(0, 0);
  });
  msuite.add_variable_to_average("U", accessors::U);
  msuite.add_variable_to_average("delta(x - x0)", [=](const metropolis &sim) {
    const double x = sim.positions()(0, 0);
    return (x < x0 + dx && x > x0 - dx) ? 1 : 0;
  });

  msuite.simulate(nsteps);

  if(taskid == 0) {
    auto averages = msuite.averages();

    const double exp_x = averages["x"];
    const double exp_xsq = averages["x^2"];
    cout << "<x>      =     " << exp_x << '\n';
    cout << "<x^2>    =     " << exp_xsq << '\n';
    cout << "kT/k     =     " << kB * T / sk << '\n';
    cout << "Delta x  =     " << sqrt(exp_xsq - exp_x*exp_x) << '\n';
    cout << "<E>      =     " << averages["U"] << '\n';
    cout << "kT / 2   =     " << kB * T / 2.0 << '\n';
    cout << "Z        =     " << (2.0 * dx * exp(-u0 / (kB * T))
                                  / averages["delta(x - x0)"]) << '\n';
    cout << "Z (an)   =     " << sqrt(2.0 * M_PI * kB * T / sk) << '\n';

    if (vm.count("plot-histogram")) {
      throw "Not yet implemented.";
    }
  }

  MPI_Finalize();

  return 0;
}