/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120210    T. Secretin       File created.
 *
 *    References
 *      Battin, R.H. An Introduction to the Mathematics and Methods of Astrodynamics,
 *          AIAA Education Series, 1999.
 *      Izzo, D. lambert_problem.h, keptoolbox.
 *
 *    Notes
 *      This code is an implementation of the method developed by Dario Izzo from ESA/ACT and
 *      publicly available at: http://keptoolbox.sourceforge.net/.
 *      After verification and validation, it was proven that this algorithm is faster and more
 *      robust than the implemented Lancaster & Blanchard and Gooding method. Notably, this method
 *      does not suffer from the near-pi singularity (pi-transfers are by nature singular).
 *
 */

#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterIzzo.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"

#include <Eigen/Geometry>

namespace tudat
{
namespace mission_segments
{

//! Execute Lambert targeting solver.
void LambertTargeterIzzo::execute( )
{
    // Call Izzo's Lambert targeting routine.
    solveLambertProblemIzzo( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                             timeOfFlight, gravitationalParameter, cartesianVelocityAtDeparture,
                             cartesianVelocityAtArrival, isRetrograde_, convergenceTolerance_,
                             maximumNumberOfIterations_ );
}

//! Get radial velocity at departure.
double LambertTargeterIzzo::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterIzzo::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterIzzo::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterIzzo::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival
            = angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterIzzo::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  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 NON INFRINGEMENT. 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.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/heat_flow.h>
#include <cinolib/laplacian.h>
#include <cinolib/vertex_mass.h>
#include <cinolib/linear_solvers.h>
#include <Eigen/Sparse>

namespace cinolib
{

template<class M, class V, class E, class P>
CINO_INLINE
ScalarField heat_flow(const AbstractMesh<M,V,E,P> & m,
                      const std::vector<uint>     & heat_charges,
                      const double                  time,
                      const int                     laplacian_mode,
                      const bool                    hard_contraint_bcs)
{
    assert(heat_charges.size() > 0);

    ScalarField heat(m.num_verts());

    Eigen::SparseMatrix<double> L   = laplacian(m, laplacian_mode);
    Eigen::SparseMatrix<double> MM  = mass_matrix(m);
    Eigen::VectorXd             rhs = Eigen::VectorXd::Zero(m.num_verts());

    if (hard_contraint_bcs) // heat flow as a boundary problem (charges do not lose heat)
    {
        std::map<uint,double> bcs;
        for(uint vid: heat_charges) bcs[vid] = 1.0;
        solve_square_system_with_bc(MM - time * L, rhs, heat, bcs);
    }
    else // heat flow as a diffusion problem (charges lose heat)
    {
        for(uint vid : heat_charges) rhs[vid] = 1.0;
        solve_square_system(MM - time * L, rhs, heat);
    }

    return heat;
}

}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2014 The University of Sydney
// 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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.

/// @author vsevolod vlaskine

#include <Eigen/Geometry>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include "polygon.h"

namespace snark {

template < typename C > static Eigen::Vector3d normal_impl( const C& corners )
{
    const Eigen::Vector3d& cross = ( corners[1] - corners[0] ).cross( corners[0] - corners[2] );
    return cross / cross.norm();
}

template < typename C > static Eigen::Vector3d projection_impl( const C& corners, const Eigen::Vector3d& rhs )
{
    const Eigen::Vector3d& n = normal_impl( corners );
    return rhs - n * ( rhs - corners[0] ).dot( n );
}

// See https://blogs.msdn.microsoft.com/rezanour/2011/08/07/barycentric-coordinates-and-point-in-triangle-tests
static inline bool is_inside( const Eigen::Vector3d& a, const Eigen::Vector3d& b, const Eigen::Vector3d& c, const Eigen::Vector3d& p )
{
    const Eigen::Vector3d& u = b - a;
    const Eigen::Vector3d& v = c - a;
    const Eigen::Vector3d& w = p - a;

    const Eigen::Vector3d& v_cross_w = v.cross( w );
    const Eigen::Vector3d& v_cross_u = v.cross( u );

    if( v_cross_w.dot( v_cross_u ) < 0 ) { return false; }

    const Eigen::Vector3d& u_cross_w = u.cross( w );
    const Eigen::Vector3d& u_cross_v = -v_cross_u;

    if( u_cross_w.dot( u_cross_v ) < 0 ) { return false; }

    double denom = u_cross_v.norm();
    double r = v_cross_w.norm();
    double t = u_cross_w.norm();

    return( r + t <= denom );
}

template < typename C > static bool includes_impl( const C& corners, const Eigen::Vector3d& rhs )
{
    for( std::size_t i = 2; i < corners.size(); ++i )
    {
        if( is_inside( corners[0], corners[i-1], corners[i], rhs ) ) { return true; }
    }
    return false;
}

static inline double distance_to_line( const Eigen::Vector3d from, const Eigen::Vector3d& to, const Eigen::Vector3d& point )
{
    typedef Eigen::ParametrizedLine< double, 3 > line_t;
    line_t line( from, ( to - from ).normalized() );
    Eigen::Vector3d projection = line.projection( point );
    bool is_between = ( ( projection - from ).squaredNorm() + ( projection - to ).squaredNorm() ) <= ( to - from ).squaredNorm();
    return is_between ? ( projection - point ).norm() : std::min( ( from - point ).norm(), ( to - point ).norm() );
}

double convex_polygon::distance_from_border_to( const Eigen::Vector3d& rhs ) const
{
    double distance = distance_to_line( corners.front(), corners.back(), rhs );
    for( unsigned int i = 1; i < corners.size(); ++i )
    {
        double d = distance_to_line( corners[ i - 1 ], corners[i], rhs );
        if( d < distance ) { distance = d; }
    }
    return distance;
}

Eigen::Vector3d convex_polygon::normal() const { return normal_impl( corners ); }

bool convex_polygon::is_valid() const
{
    if( corners.size() < 3 ) { return false; }
    COMMA_THROW( comma::exception, "todo" );
    for( std::size_t i = 1; i < corners.size(); ++i )
    {
        // todo
    }
    return true;
}

Eigen::Vector3d convex_polygon::projection_of( const Eigen::Vector3d& rhs ) const { return projection_impl( corners, rhs ); }

bool convex_polygon::includes( const Eigen::Vector3d& rhs ) const { return includes_impl( corners, rhs ); }

Eigen::Vector3d triangle::normal() const { return normal_impl( corners ); }

bool triangle::is_valid() const
{
    if( corners.size() != 3 ) { return false; }
    const Eigen::Vector3d& cross = ( corners[1] - corners[0] ).cross( corners[0] - corners[2] );
    return !comma::math::equal( cross.norm(), 0 );
}

Eigen::Vector3d triangle::projection_of( const Eigen::Vector3d& rhs ) const { return projection_impl( corners, rhs ); }

bool triangle::includes( const Eigen::Vector3d& rhs ) const { return includes_impl( corners, rhs ); }

double triangle::circumscribing_radius() const
{
    COMMA_THROW( comma::exception, "todo" );
    const Eigen::Vector3d& a = corners[1] - corners[0];
    const Eigen::Vector3d& b = corners[2] - corners[1];
    const Eigen::Vector3d& c = corners[0] - corners[2];
    return a.norm() / ( std::sqrt( 1 - b.dot( c ) / ( b.squaredNorm() * c.squaredNorm() ) ) * 2 );
}

} // namespace snark {


#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <QDebug>
#include <QPrinter>

#include <cmath>
#include <iostream>

#include "MotionModes/crab.h"
#include "MotionModes/motion_mode.h"
#include "MotionModes/tangent_aligned.h"
#include "wheel.h"

#include <dlib/global_optimization.h>

MainWindow::MainWindow(QWidget* parent) : QMainWindow(parent), ui(new Ui::MainWindow), scene(new CustomScene)
{
  ui->setupUi(this);

  ui->graphicsView->setScene(scene);
  new QGraphicsViewZoom(ui->graphicsView);

  Eigen::MatrixX2d cp1, cp2;
  cp1.resize(4, 2);
  cp2.resize(4, 2);

  cp1 << -1, 0, -1, -4. / 3, 1, -4. / 3, 1.5, -0.5;
  cp1 *= 300;
  cp2 << 1.5, -0.5, 1, 2, 2, 1, 3, 1;
  cp2 *= 150;
  cp2.col(0) *= 2;

  curve_1 = new qCurve(Bezier::Curve(cp1).splitCurve(0.5).second, ui, 1);
  curve_2 = new qCurve(cp2, ui, 2);

  curve_2->applyContinuity(*curve_1, {1.4});

  curve_1->elevateOrder();
  curve_1->elevateOrder();
  curve_1->elevateOrder();

  curve_2->elevateOrder();
  curve_2->elevateOrder();
  curve_2->elevateOrder();

  scene->addItem(curve_1);
  scene->addItem(curve_2);

  curve_1->setDraw_control_points(true);
  curve_2->setDraw_control_points(true);

  ui->graphicsView->centerOn(scene->itemsBoundingRect().center());

  // plots
  ui->plot2->yAxis2->setVisible(true);

  // add title layout element:
  //  ui->plot1->plotLayout()->insertRow(0);
  //  ui->plot2->plotLayout()->insertRow(0);
  //  ui->plot3->plotLayout()->insertRow(0);
  //  ui->plot1->setFont(QFont("Times", 12));
  //  ui->plot2->setFont(QFont("Times", 12));
  //  ui->plot3->setFont(QFont("Times", 12));
  //  ui->plot1->plotLayout()->addElement(0, 0, new QCPTextElement(ui->plot1, "Speed limit and speed profiles"));
  //  ui->plot2->plotLayout()->addElement(0, 0, new QCPTextElement(ui->plot2, "Angular velocities"));
  //  ui->plot3->plotLayout()->addElement(0, 0, new QCPTextElement(ui->plot3, "Vehicle orientation and steering
  //  angles"));

  // set labels:
  ui->plot1->xAxis->setLabelFont(QFont("Times", 10));
  ui->plot2->xAxis->setLabelFont(QFont("Times", 10));
  ui->plot1->xAxis->setLabel("Distance along path [m]");
  ui->plot2->xAxis->setLabel("Distance along path [m]");

  ui->plot1->yAxis->setLabelFont(QFont("Times", 10));
  ui->plot2->yAxis->setLabelFont(QFont("Times", 10));
  ui->plot2->yAxis2->setLabelFont(QFont("Times", 10));
  ui->plot1->yAxis->setLabel("Speed [m/s]");
  ui->plot2->yAxis->setLabel("Vehicle orientation and wheel steering angle [°]");
  ui->plot2->yAxis2->setLabel("Angular velocity [°/s]");

  // set ticks:
  ui->plot1->xAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot2->xAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot1->yAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot2->yAxis->setTickLabelFont(QFont("Times", 10));
  ui->plot2->yAxis2->setTickLabelFont(QFont("Times", 10));

  // show legend
  ui->plot1->legend->setVisible(true);
  ui->plot2->legend->setVisible(true);
  ui->plot1->legend->setFont(QFont("Times", 10));
  ui->plot2->legend->setFont(QFont("Times", 10));
  ui->plot1->legend->setRowSpacing(-5);
  ui->plot2->legend->setRowSpacing(-5);
  ui->plot1->axisRect()->insetLayout()->setInsetAlignment(0, Qt::AlignBottom | Qt::AlignRight);
  ui->plot2->axisRect()->insetLayout()->setInsetAlignment(0, Qt::AlignTop | Qt::AlignRight);

  // open in new window
  //  ui->plot1->setParent(nullptr);
  //  ui->plot2->setParent(nullptr);
  ui->plot1->setMinimumSize(400, 300);
  ui->plot2->setMinimumSize(400, 300);
  ui->plot1->show();
  ui->plot2->show();

  auto saveAs = []() {
    QFileDialog dialog(nullptr, "Export PDF");
    dialog.setNameFilter("PDF (*.pdf)");
    dialog.setFileMode(QFileDialog::AnyFile);
    dialog.setAcceptMode(QFileDialog::AcceptSave);
    dialog.setDefaultSuffix("pdf");
    if (dialog.exec())
      return dialog.selectedFiles().first();
    return QString();
  };

  connect(ui->plot1, &QCustomPlot::mouseRelease, this, [&](QMouseEvent* e) {
    if (e->button() == Qt::RightButton)
    {
      auto file_path = saveAs();
      if (file_path == QString())
        return;
      ui->plot1->savePdf(file_path, 0, 0, QCP::epNoCosmetic);
    }
  });
  connect(ui->plot2, &QCustomPlot::mouseRelease, this, [&](QMouseEvent* e) {
    if (e->button() == Qt::RightButton)
    {
      auto file_path = saveAs();
      if (file_path == QString())
        return;
      ui->plot2->savePdf(file_path, 0, 0, QCP::epNoCosmetic);
    }
  });

  //    QPrinter printer(QPrinter::HighResolution);
  //    printer.setPageSize(
  //        QPageSize(scene->itemsBoundingRect().size().scaled(90, 90, Qt::AspectRatioMode::KeepAspectRatio),
  //                  QPageSize::Unit::Point));
  //    printer.setOrientation(QPrinter::Portrait);
  //    printer.setOutputFormat(QPrinter::PdfFormat);
  //    printer.setFullPage(true);
  //    auto file_path = saveAs();
  //    if (file_path == QString())
  //      return;
  //    printer.setOutputFileName(file_path);

  //    QPainter p;

  //    if (!p.begin(&printer))
  //    {
  //      qDebug() << "Error!";
  //      return;
  //    }

  //    this->scene->render(&p);
  //    p.end();

  //    int sl = file_path.lastIndexOf('/') + 1;
  //    QString file_path2 = file_path.left(sl) + "angle" + file_path.right(6);
  //    ui->plot2->savePdf(file_path2, 0, 0, QCP::epNoCosmetic);
  //    QString file_path3 = file_path.left(sl) + "speed" + file_path.right(6);
  //    ui->plot1->savePdf(file_path3, 0, 0, QCP::epNoCosmetic);
  //    // ovdje dodati export i ostalih

  ui->plot1->setInteraction(QCP::iRangeDrag, true);
  ui->plot2->setInteraction(QCP::iRangeDrag, true);

  ui->plot1->axisRect()->setRangeDrag(Qt::Horizontal);
  ui->plot2->axisRect()->setRangeDrag(Qt::Horizontal);

  ui->plot1->setInteraction(QCP::iRangeZoom, true);
  ui->plot2->setInteraction(QCP::iRangeZoom, true);

  ui->plot1->axisRect()->setRangeZoom(Qt::Horizontal);
  ui->plot2->axisRect()->setRangeZoom(Qt::Horizontal);

  // curves
  connect(ui->optimize, &QPushButton::pressed, this, &MainWindow::applyContinuity);
  //  connect(ui->alpha1, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->alpha2, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->n1, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->n2, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->mm1, qOverload<int>(&QComboBox::currentIndexChanged), this, &MainWindow::applyContinuity);
  //  connect(ui->mm2, qOverload<int>(&QComboBox::currentIndexChanged), this, &MainWindow::applyContinuity);

  // vehicle tab
  ui->vehicle->header()->resizeSection(0, 150);
  ui->vehicle->header()->setSectionResizeMode(1, QHeaderView::Stretch);
  ui->vehicle->header()->setStretchLastSection(false);

  static auto addWheel = [&]() {
    Wheel* temp = new Wheel(ui->vehicle->topLevelItemCount(), ui->vehicle);
    ui->vehicle->addTopLevelItem(temp);
    temp->expand();
    connect(temp, &Wheel::wheelChanged, this, &MainWindow::updatePlot);
  };

  static auto removeWheel = [&]() { delete ui->vehicle->takeTopLevelItem(ui->vehicle->topLevelItemCount() - 1); };

  for (uint k = 0; k < ui->wheel_num->value(); k++)
    addWheel();
  connect(scene, &QGraphicsScene::changed, this, &MainWindow::updatePlot);

  connect(ui->wheel_num, qOverload<int>(&QSpinBox::valueChanged), this, [&](int n) {
    while (n > ui->vehicle->topLevelItemCount())
      addWheel();
    while (n < ui->vehicle->topLevelItemCount())
      removeWheel();
    updatePlot();
  });

  //  curve_1->setVisible(false);
  //  curve_2->setVisible(false);

  //  ui->wheel_num->setValue(6);

  //  static const QVector<QColor> matlab_colors(
  //      {{0, 114, 189}, {217, 83, 25}, {237, 177, 32}, {126, 47, 142}, {119, 172, 48}, {77, 190, 238}, {162, 20,
  //      47}});
  //  QPainterPath vehicle_frame;

  //  vehicle_frame = QPainterPath();
  //  vehicle_frame.moveTo(50, 0);
  //  vehicle_frame.lineTo(0, 0);
  //  vehicle_frame.lineTo(0, -50);
  //  vehicle_frame.lineTo(-3,-47);
  //  vehicle_frame.lineTo(0, -50);
  //  vehicle_frame.lineTo(3,-47);

  //  QVector<QString> w;
  //  w.push_back(u8"w\u2081");
  //  w.push_back(u8"w\u2082");
  //  w.push_back(u8"w\u2083");
  //  w.push_back(u8"w\u2084");
  //  w.push_back(u8"w\u2085");
  //  w.push_back(u8"w\u2086");

  //    scene->addPath(vehicle_frame, QPen(matlab_colors[0], 3));
  //    for(int k = 0; k < ui->wheel_num->value(); k++)
  //    {
  //      auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));
  //      auto R = wheel->getR();
  //      scene->addRect(QRect(R.x()*10-100, -(R.y()*10-50), 200, -100), QPen(matlab_colors[k+1], 1));
  //      scene->addLine(0,0,R.x()*10, -R.y()*10);
  //      auto x = scene->addText(w[k], QFont("Times", 24));
  //      x->setPos(R.x()*10, -R.y()*10);
  //    }

  //  scene->update();

  //  {
  //      QPrinter printer( QPrinter::HighResolution );
  //      printer.setPageSize(QPageSize(qCurve(cp2*5).boundingRect().size(), QPageSize::Unit::Point));
  //      printer.setOrientation( QPrinter::Portrait );
  //      printer.setOutputFormat( QPrinter::PdfFormat );
  //      printer.setFullPage(true);
  //      printer.setOutputFileName( "/home/mirko/Pictures/motion_modes/" + qCurve::getName());

  //      QPainter p;

  //      if( !p.begin( &printer ) )
  //      {
  //          qDebug() << "Error!";
  //          return;
  //      }
  //      this->scene->render( &p );
  //      p.end();
  //  }

  //  auto dC = curve_1->derivativeAt(1);
  //  ui->alpha2->setValue(std::atan2(dC.y(), dC.x()) * 180 / M_PI);
  //  ui->mm2->setCurrentIndex(2);
}

MainWindow::~MainWindow() { delete ui; }

void MainWindow::applyContinuity()
{
  Romb::MotionModes::MotionMode *mm_1, *mm_2;
  double alpha_1 = ui->alpha1->value() * M_PI / 180;
  double alpha_2 = ui->alpha2->value() * M_PI / 180;
  double n_1 = ui->n1->value();
  double n_2 = ui->n2->value();

  switch (ui->mm1->currentIndex())
  {
  case 0:
    mm_1 = new Romb::MotionModes::TangentAligned(alpha_1);
    break;
  case 1:
    mm_1 = new Romb::MotionModes::Crab(alpha_1);
    break;
  case 2:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, n_1);
    break;
  default:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, -n_1);
    break;
  }

  switch (ui->mm2->currentIndex())
  {
  case 0:
    mm_2 = new Romb::MotionModes::TangentAligned(alpha_2);
    break;
  case 1:
    mm_2 = new Romb::MotionModes::Crab(alpha_2);
    break;
  case 2:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, n_2);
    break;
  default:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, -n_2);
    break;
  }

  dlib::thread_pool tp(std::thread::hardware_concurrency());
  std::function<void(std::vector<double>, Bezier::Curve&, Bezier::Curve&)> applyParams;
  std::function<double(std::vector<double>)> evaluate = [&](std::vector<double> params) {
    Bezier::Curve temp1 = *curve_1;
    Bezier::Curve temp2 = *curve_2;
    applyParams(params, temp1, temp2);

    Data sim = calculateData(temp1, temp2, true);

    auto time = [](QVector<double> v, QVector<double> s) {
      double res{0.0};
      for (int k = 1; k < s.size(); k++)
        res += 2 * (s[k] - s[k - 1]) / (v[k] + v[k - 1]);
      return res;
    };

    return time(sim.v_sim1, sim.s1) + time(sim.v_sim2, sim.s2);
  };

  constexpr std::chrono::seconds opt_max_duration(15);

  if (mm_1->type == "aligned" && mm_2->type == "aligned")
  {
    ui->alpha2->setValue(alpha_1);
    applyParams = [](std::vector<double> params, Bezier::Curve& curve_1, Bezier::Curve& curve_2) {
      curve_2.applyContinuity(curve_1, {params[0], params[1], params[2]});
    };
    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2) {
          return evaluate({x0, x1, x2});
        },
        {0.01, 0, 0}, {5, 5, 5}, opt_max_duration);

    applyParams(std::vector<double>(result.x.begin(), result.x.end()), *curve_1, *curve_2);
  }
  if ((mm_1->type == "aligned" || mm_1->type == "exponential_1") && mm_2->type == "crab")
  {
    auto dc = curve_1->derivativeAt(1);
    ui->alpha2->setValue((std::atan2(dc.y(), dc.x()) + alpha_1) * 180 / M_PI);
    applyParams = [](std::vector<double> params, Bezier::Curve& curve_1, Bezier::Curve& curve_2) {
      auto dc = curve_1.derivativeAt(1);
      curve_1.manipulateControlPoint(curve_1.order() - 1, curve_1.endPoints().second - params[0] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 2, curve_1.endPoints().second - params[1] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 3, curve_1.endPoints().second - params[2] * dc);

      curve_2.manipulateControlPoint(0, curve_1.endPoints().second);
      curve_2.manipulateControlPoint(1, curve_1.endPoints().second + params[3] * dc);
      curve_2.manipulateControlPoint(2, curve_1.endPoints().second + params[4] * dc);
    };

    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2, double x3, double x4) {
          return evaluate({x0, x1, x2, x3, x4});
        },
        {0.1, 0, 0, 0.1, 0}, {5, 5, 5, 5, 5}, opt_max_duration);
    applyParams({result.x(0), result.x(1), result.x(2), result.x(3), result.x(4)}, *curve_1, *curve_2);
  }
  if ((mm_2->type == "aligned" || mm_2->type == "exponential_2") && mm_1->type == "crab")
  {
    auto dc = curve_1->derivativeAt(1);
    ui->alpha2->setValue((std::atan2(dc.y(), dc.x()) + alpha_1) * 180 / M_PI);
    applyParams = [](std::vector<double> params, Bezier::Curve& curve_2, Bezier::Curve& curve_1) {
      auto dc = curve_1.derivativeAt(0);
      curve_1.manipulateControlPoint(curve_1.order() - 1, curve_1.endPoints().second - params[0] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 2, curve_1.endPoints().second - params[1] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 3, curve_1.endPoints().second - params[2] * dc);

      curve_2.manipulateControlPoint(0, curve_1.endPoints().second);
      curve_2.manipulateControlPoint(1, curve_1.endPoints().second + params[3] * dc);
      curve_2.manipulateControlPoint(2, curve_1.endPoints().second + params[4] * dc);
    };

    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2, double x3, double x4) {
          return evaluate({x0, x1, x2, x3, x4});
        },
        {0.1, 0, 0, 0.1, 0}, {5, 5, 5, 5, 5}, opt_max_duration);
    applyParams({result.x(0), result.x(1), result.x(2), result.x(3), result.x(4)}, *curve_1, *curve_2);
  }

  if (mm_1->type == "aligned" && mm_2->type == "exponential_2")
  {
    ui->alpha2->setValue(ui->alpha1->value());
    applyParams = [&](std::vector<double> params, Bezier::Curve& curve_1, Bezier::Curve& curve_2) {
      auto dc = curve_1.derivativeAt(1);
      curve_1.manipulateControlPoint(curve_1.order() - 1, curve_1.endPoints().second - params[0] * dc);
      curve_1.manipulateControlPoint(curve_1.order() - 2, curve_1.endPoints().second - params[1] * dc);

      curve_2.manipulateControlPoint(0, curve_1.endPoints().second);
      curve_2.manipulateControlPoint(1, curve_1.endPoints().second + params[2] * dc);
      curve_2.manipulateControlPoint(2, curve_1.endPoints().second + params[3] * dc);

      double beta_1 = curve_1.derivativeAt(1).norm() / curve_2.derivativeAt(0).norm();
      double beta_2 = curve_1.derivativeAt(1).norm() / curve_2.derivativeAt(0).norm();

      double dC3_scale = n_2 * n_2 * (beta_1 * beta_1 * beta_1);

      double beta_3 = (curve_2.derivativeAt(3, 0).norm() * dC3_scale -
                       beta_1 * beta_1 * beta_1 * curve_2.derivativeAt(2, 0).norm() -
                       2 * beta_1 * beta_2 * curve_2.derivativeAt(2, 0).norm()) /
                      curve_2.derivativeAt(0).norm();

      curve_2.applyContinuity(curve_1, {beta_1, beta_2, beta_3});
    };

    auto result = dlib::find_min_global(
        tp,
        [&](double x0, double x1, double x2, double x3) {
          return evaluate({x0, x1, x2, x3});
        },
        {0.0, 0, 0.0, 0.0}, {3, 3, 3, 3}, opt_max_duration);
    applyParams({result.x(0), result.x(1), result.x(2), result.x(3)}, *curve_1, *curve_2);
  }

  scene->update();
  updatePlot();
}

void MainWindow::updatePlot()
{
  const QString _max(u8"\u2098\u2090\u2093");
  const QString _sim(u8"\u209b\u1d62\u2098");

  Data data = calculateData(*curve_1, *curve_2);
  int N = ui->vehicle->topLevelItemCount();
  static const QVector<QColor> matlab_colors(
      {{0, 114, 189}, {217, 83, 25}, {237, 177, 32}, {126, 47, 142}, {119, 172, 48}, {77, 190, 238}, {162, 20, 47}});

  ////// PLOTANJE
  QPen pen;
  pen.setStyle(Qt::DashLine);
  static QCPItemStraightLine* infLine1 = new QCPItemStraightLine(ui->plot1);
  static QCPItemStraightLine* infLine2 = new QCPItemStraightLine(ui->plot2);
  infLine1->setPen(pen);
  infLine2->setPen(pen);
  infLine1->point1->setCoords(data.s1.back(), 0); // location of point 1 in plot coordinate
  infLine1->point2->setCoords(data.s1.back(), 1); // location of point 2 in plot coordinate
  infLine2->point1->setCoords(data.s1.back(), 0); // location of point 1 in plot coordinate
  infLine2->point2->setCoords(data.s1.back(), 1); // location of point 2 in plot coordinate

  /// plot brzina
  {
    constexpr int n_v = 4;
    while (ui->plot1->graphCount() < 4 * N + n_v)
      ui->plot1->addGraph();
    while (ui->plot1->graphCount() > 4 * N + n_v)
      ui->plot1->removeGraph(ui->plot1->graphCount() - 1);

    auto g_v1 = ui->plot1->graph(0);
    auto g_v2 = ui->plot1->graph(1);
    auto g_v_sim1 = ui->plot1->graph(2);
    auto g_v_sim2 = ui->plot1->graph(3);

    g_v1->setData(data.s1, data.v1, true);
    g_v2->setData(data.s2, data.v2, true);
    g_v_sim1->setData(data.s1, data.v_sim1, true);
    g_v_sim2->setData(data.s2, data.v_sim2, true);

    g_v1->setName("v" + _max);
    g_v2->removeFromLegend();
    g_v_sim1->setName("v" + _sim);
    g_v_sim2->removeFromLegend();

    pen.setStyle(Qt::SolidLine);
    pen.setColor(matlab_colors[0]);
    pen.setWidth(3);
    g_v1->setPen(pen);
    g_v2->setPen(pen);
    pen.setStyle(Qt::DashLine);
    g_v_sim1->setPen(pen);
    g_v_sim2->setPen(pen);

    for (int k = 0; k < N; k++)
    {
      auto g_w_v1 = ui->plot1->graph(n_v + 4 * k);
      auto g_w_v2 = ui->plot1->graph(n_v + 4 * k + 1);
      auto g_w_v_sim1 = ui->plot1->graph(n_v + 4 * k + 2);
      auto g_w_v_sim2 = ui->plot1->graph(n_v + 4 * k + 3);

      g_w_v1->setName("w" + QString::number(k + 1) + _max);
      g_w_v2->removeFromLegend();
      g_w_v_sim1->setName("w" + QString::number(k + 1) + _sim);
      g_w_v_sim2->removeFromLegend();

      g_w_v1->setData(data.s1, data.v_w1[k], true);
      g_w_v2->setData(data.s2, data.v_w2[k], true);
      g_w_v_sim1->setData(data.s1, data.v_w_sim1[k], true);
      g_w_v_sim2->setData(data.s2, data.v_w_sim2[k], true);

      pen.setWidth(1);
      pen.setColor(matlab_colors[k + 1]);
      pen.setStyle(Qt::SolidLine);
      g_w_v1->setPen(pen);
      g_w_v2->setPen(pen);
      pen.setStyle(Qt::DashLine);
      g_w_v_sim1->setPen(pen);
      g_w_v_sim2->setPen(pen);
    }

    ui->plot1->rescaleAxes();
    ui->plot1->yAxis->scaleRange(1.05);
    ui->plot1->replot();
  }

  /// plot kutnih brzina
  {
    constexpr int n_w = 4;
    while (ui->plot2->graphCount() < 4 * N + n_w)
    {
      ui->plot2->addGraph();
      ui->plot2->addGraph(ui->plot2->xAxis, ui->plot2->yAxis2);
    }
    while (ui->plot2->graphCount() > 4 * N + n_w)
      ui->plot2->removeGraph(ui->plot2->graphCount() - 1);

    auto g_theta1 = ui->plot2->graph(0);
    auto g_theta2 = ui->plot2->graph(2);
    auto g_w_sim1 = ui->plot2->graph(1);
    auto g_w_sim2 = ui->plot2->graph(3);

    g_theta1->setName(u8"\u03b8");
    g_theta2->removeFromLegend();
    g_w_sim1->setName(u8"\u03c9" + _sim);
    g_w_sim2->removeFromLegend();

    g_theta1->setData(data.s1, data.theta1, true);
    g_theta2->setData(data.s2, data.theta2, true);
    g_w_sim1->setData(data.s1, data.w_sim1, true);
    g_w_sim2->setData(data.s2, data.w_sim2, true);

    pen.setStyle(Qt::SolidLine);
    pen.setColor(matlab_colors[0]);
    pen.setWidth(3);
    g_theta1->setPen(pen);
    g_theta2->setPen(pen);
    pen.setStyle(Qt::DashLine);
    g_w_sim1->setPen(pen);
    g_w_sim2->setPen(pen);

    for (int k = 0; k < N; k++)
    {
      auto g_delta1 = ui->plot2->graph(n_w + 4 * k + 0);
      auto g_delta2 = ui->plot2->graph(n_w + 4 * k + 2);
      auto g_w_w_sim1 = ui->plot2->graph(n_w + 4 * k + 1);
      auto g_w_w_sim2 = ui->plot2->graph(n_w + 4 * k + 3);

      g_delta1->setName(u8"\u03b4" + QString::number(k + 1));
      g_delta2->removeFromLegend();
      g_w_w_sim1->setName(u8"\u03c9w" + QString::number(k + 1) + _sim);
      g_w_w_sim2->removeFromLegend();

      g_delta1->setData(data.s1, data.delta1[k], true);
      g_delta2->setData(data.s2, data.delta2[k], true);
      g_w_w_sim1->setData(data.s1, data.w_w_sim1[k], true);
      g_w_w_sim2->setData(data.s2, data.w_w_sim2[k], true);

      pen.setStyle(Qt::SolidLine);
      pen.setWidth(1);
      pen.setColor(matlab_colors[k + 1]);
      g_delta1->setPen(pen);
      g_delta2->setPen(pen);
      pen.setStyle(Qt::DashLine);
      g_w_w_sim1->setPen(pen);
      g_w_w_sim2->setPen(pen);
    }

    ui->plot2->rescaleAxes();
    ui->plot2->yAxis->scaleRange(1.05);
    ui->plot2->yAxis2->scaleRange(1.05);
    ui->plot2->replot();
  }

  auto print = [](double x) {
    std::ostringstream res;
    res.precision(3);

    if (x < 0)
      res << "{" << std::fixed << x << "}";
    else
      res << "  " << std::fixed << x << " ";
    return res;
  };

  std::stringstream bezier1, bezier2;
  auto cp1 = curve_1->controlPoints();
  auto cp2 = curve_1->controlPoints();
  for (auto& p : cp1)
  {
    p = {std::round(p.x() * 10) / 1000., std::round(p.y() * 10) / 1000.};
    bezier1 << "( " << print(p.x()).str() << ",&\\ " << print(p.y()).str() << " ) \\\\\n";
  }
  for (auto& p : cp1)
  {
    p = {std::round(p.x() * 10) / 1000., std::round(p.y() * 10) / 1000.};
    bezier2 << "( " << print(p.x()).str() << ",&\\ " << print(p.y()).str() << " ) \\\\\n";
  }
  cp1.back() = {std::round(cp1.back().x() * 10) / 1000., std::round(cp1.back().y() * 10) / 1000.};
  bezier1 << "( " << print(cp1.back().x()).str() << ",&\\ " << print(cp1.back().y()).str() << " )";
  cp2.back() = {std::round(cp2.back().x() * 10) / 1000., std::round(cp2.back().y() * 10) / 1000.};
  bezier2 << "( " << print(cp2.back().x()).str() << ",&\\ " << print(cp2.back().y()).str() << " )";

  ui->bezier1->setText(QString::fromStdString(bezier1.str()));
  ui->bezier2->setText(QString::fromStdString(bezier2.str()));
}

MainWindow::Data MainWindow::calculateData(const Bezier::Curve& curve_1, const Bezier::Curve& curve_2, bool partial)
{
  ///////// DATA
  ///
  ///
  ///
  ///
  constexpr double step = 0.005;
  constexpr int samples = static_cast<int>(1. / step + 1);
  constexpr double a = 0.5;
  int N = ui->vehicle->topLevelItemCount();
  Romb::MotionModes::MotionMode *mm_1, *mm_2;
  double alpha_1 = ui->alpha1->value() * M_PI / 180;
  double alpha_2 = ui->alpha2->value() * M_PI / 180;
  double n_1 = ui->n1->value();
  double n_2 = ui->n2->value();
  double v_max_1 = ui->vmax1->value() * 100;
  double v_max_2 = ui->vmax2->value() * 100;
  switch (ui->mm1->currentIndex())
  {
  case 0:
    mm_1 = new Romb::MotionModes::TangentAligned(alpha_1);
    break;
  case 1:
    mm_1 = new Romb::MotionModes::Crab(alpha_1);
    break;
  case 2:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, n_1);
    break;
  default:
    mm_1 = new Romb::MotionModes::Exponential(alpha_1, -n_1);
    break;
  }
  switch (ui->mm2->currentIndex())
  {
  case 0:
    mm_2 = new Romb::MotionModes::TangentAligned(alpha_2);
    break;
  case 1:
    mm_2 = new Romb::MotionModes::Crab(alpha_2);
    break;
  case 2:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, n_2);
    break;
  default:
    mm_2 = new Romb::MotionModes::Exponential(alpha_1, -n_2);
    break;
  }

  Data results(N, samples, partial);

  auto& t = results.t;
  auto& s1 = results.s1;
  auto& s2 = results.s2;
  auto& v1 = results.v1;
  auto& v2 = results.v2;
  auto& v_sim1 = results.v_sim1;
  auto& v_sim2 = results.v_sim2;
  auto& w_sim1 = results.w_sim1;
  auto& w_sim2 = results.w_sim2;
  auto& theta1 = results.theta1;
  auto& theta2 = results.theta2;
  auto& delta1 = results.delta1;
  auto& delta2 = results.delta2;
  auto& v_w1 = results.v_w1;
  auto& v_w2 = results.v_w2;
  auto& v_w_sim1 = results.v_w_sim1;
  auto& v_w_sim2 = results.v_w_sim2;
  auto& w_w_sim1 = results.w_w_sim1;
  auto& w_w_sim2 = results.w_w_sim2;

  double S = curve_1.length() / 100;

  /////////// izracun limita brzine
  ///
  ///
  ///
  ///
  for (int idx = 0; idx < samples; idx++)
  {
    t[idx] = idx * step;
    s1[idx] = curve_1.length(t[idx]) / 100;
    s2[idx] = S + curve_2.length(t[idx]) / 100;
    v1[idx] = v_max_1;
    v2[idx] = v_max_2;

    for (int k = 0; k < N; k++)
    {
      auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));

      // single wheel limit #1
      v_w1[k][idx] = wheel->vMax(curve_1, mm_1, t[idx]);
      if (v_w1[k][idx] < v1[idx])
        v1[idx] = v_w1[k][idx];

      // single wheel limit #2
      v_w2[k][idx] = wheel->vMax(curve_2, mm_2, t[idx]);
      if (v_w2[k][idx] < v2[idx])
        v2[idx] = v_w2[k][idx];
    }
    v1[idx] /= 100;
    v2[idx] /= 100;

    // combined speed limit
    for (int k = 0; k < N; k++)
    {
      v_w1[k][idx] = static_cast<Wheel*>(ui->vehicle->topLevelItem(k))->Rv(curve_1, mm_1, t[idx]) * v1[idx];
      v_w2[k][idx] = static_cast<Wheel*>(ui->vehicle->topLevelItem(k))->Rv(curve_2, mm_2, t[idx]) * v2[idx];
    }
  }

  //////// simulacija brzine
  ///
  ///
  ///
  ///
  QVector<double> v_full = v1;
  QVector<double> s_full = s1;

  v_full.append(v2);
  s_full.append(s2);

  int last_idx = std::numeric_limits<int>::infinity();

  auto fill_sim = [&](double v, int idx) {
    auto fill_all = [&](double v, int idx) {
      auto& v_w_sim = idx < samples ? v_w_sim1 : v_w_sim2;
      auto& curve = idx < samples ? curve_1 : curve_2;
      auto& mm = idx < samples ? mm_1 : mm_2;
      idx = idx < samples ? idx : idx - samples;

      for (int k = 0; k < N; k++)
      {
        auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));
        v_w_sim[k][idx] = wheel->Rv(curve, mm, t[idx]) * v;
      }
    };

    for (int k = last_idx + 1; k <= idx; k++)
    {
      if (k == samples)
      {
        v_sim2[0] = v_sim1[k - 1];
        fill_all(v_sim2[0], k);
      }
      else
      {
        double dS = s_full[k] - s_full[k - 1];
        auto& v_sim = k < samples ? v_sim1 : v_sim2;
        int temp_idx = k < samples ? k : k - samples;
        if (v > v_sim[temp_idx - 1])
          v_sim[temp_idx] = std::min(v, std::sqrt(v_sim[temp_idx - 1] * v_sim[temp_idx - 1] + 2 * a * dS));
        else
          v_sim[temp_idx] = std::max(v, std::sqrt(v_sim[temp_idx - 1] * v_sim[temp_idx - 1] - 2 * a * dS));
        fill_all(v_sim[temp_idx], k);
      }
    }

    last_idx = idx;
  };

  std::function<void(int, double, int, double)> d_and_c = [&](int idx1, double v_1, int idx3, double v_3) {
    if (idx1 + 1 == idx3)
    {
      fill_sim(v_1, idx1);
      return;
    }

    int idx2 =
        static_cast<int>(std::min_element(v_full.begin() + idx1 + 1, v_full.begin() + idx3 - 1) - v_full.begin());

    double v_reachable_1 = std::sqrt(v_1 * v_1 + 2 * a * (s_full[idx2] - s_full[idx1]));
    double v_reachable_2 = std::sqrt(v_3 * v_3 + 2 * a * (s_full[idx3] - s_full[idx2]));

    if (v_reachable_1 > std::min(v_reachable_2, v_full[idx2]))
      d_and_c(idx1, v_1, idx2, std::min({v_reachable_1, v_reachable_2, v_full[idx2]}));
    else
      fill_sim(v_1, idx1);

    if (v_reachable_2 > std::min(v_reachable_1, v_full[idx2]))
      d_and_c(idx2, std::min({v_reachable_1, v_reachable_2, v_full[idx2]}), idx3, v_3);
    else
      fill_sim(std::min({v_reachable_1, v_reachable_2, v_full[idx2]}), idx2);
  };

  d_and_c(0, 0.0, 2 * samples - 1, 0.0);
  fill_sim(0.0, 2 * samples - 1);

  // return if only speed sim is wanted
  if (partial)
    return results;

  /////////// angular velocity and orientation sim
  ///
  ///
  ///
  ///
  ///

  for (int idx = 0; idx < samples; idx++)
  {
    theta1[idx] = mm_1->phi(curve_1, t[idx]) * 180 / M_PI;
    theta2[idx] = mm_2->phi(curve_2, t[idx]) * 180 / M_PI;
    w_sim1[idx] =
        v_sim1[idx] * mm_1->phiDerived(curve_1, t[idx]) / curve_1.derivativeAt(t[idx]).norm() * 100 * 180 / M_PI;
    w_sim2[idx] =
        v_sim2[idx] * mm_2->phiDerived(curve_2, t[idx]) / curve_2.derivativeAt(t[idx]).norm() * 100 * 180 / M_PI;

    for (int k = 0; k < N; k++)
    {
      auto wheel = static_cast<Wheel*>(ui->vehicle->topLevelItem(k));
      delta1[k][idx] = wheel->delta(curve_1, mm_1, t[idx]) * 180 / M_PI;
      delta2[k][idx] = wheel->delta(curve_2, mm_2, t[idx]) * 180 / M_PI;
      w_w_sim1[k][idx] = v_sim1[idx] * wheel->Rw(curve_1, mm_1, t[idx]) * 100 * 180 / M_PI;
      w_w_sim2[k][idx] = v_sim2[idx] * wheel->Rw(curve_2, mm_2, t[idx]) * 100 * 180 / M_PI;
    }
  }

  return results;
}

MainWindow::Data::Data(int N, int samples, bool partial)
{
  t.resize(samples);
  s1.resize(samples);
  s2.resize(samples);
  v1.resize(samples);
  v2.resize(samples);

  v_sim1.resize(samples);
  v_sim2.resize(samples);
  v_w1.resize(N);
  v_w2.resize(N);
  v_w_sim1.resize(N);
  v_w_sim2.resize(N);
  if (!partial)
  {
    theta1.resize(samples);
    theta2.resize(samples);
    w_sim1.resize(samples);
    w_sim2.resize(samples);
    delta1.resize(N);
    delta2.resize(N);
    w_w_sim1.resize(N);
    w_w_sim2.resize(N);
  }

  for (int k = 0; k < N; k++)
  {
    v_w1[k].resize(samples);
    v_w2[k].resize(samples);
    v_w_sim1[k].resize(samples);
    v_w_sim2[k].resize(samples);
    if (!partial)
    {
      delta1[k].resize(samples);
      delta2[k].resize(samples);
      w_w_sim1[k].resize(samples);
      w_w_sim2[k].resize(samples);
    }
  }
}


/*
 * 根据2D图像和深度图生成点云
 * Author: YangQun
 * Date: 2018/5/9
 * Last Update:2018/5/9
 */
// C++ 标准库
#include <iostream>
#include <string>
#include <vector>

// 第三方库头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>

// 工程头文件
#include "generate_point_cloud.h"
#include "param_reader.h"

using namespace std;

PointCloudGenerator::PointCloudGenerator(PinHoleCamera::Ptr cam, ParameterReader::Ptr param_reader)
// viewer_("viewer")	// 初始化点云显示窗口的名字
{
	cam_ = cam;
	is_show_ = param_reader->getParam<bool>("is_show_point_cloud");
	is_save_ = param_reader->getParam<bool>("is_save_point_cloud");
	grid_size_ = param_reader->getParam<double>("voxel_grid");
	is_filter_ = param_reader->getParam<bool>("is_filter");

	if (is_filter_) {
		voxel_.setLeafSize(grid_size_, grid_size_, grid_size_);
	}

	cloud_out_.reset(new PointCloud);

	if (is_show_) {
		viewer_ = new pcl::visualization::CloudViewer("viewer");
	}
	else {
		viewer_ = nullptr;
	}
}

PointCloudGenerator::~PointCloudGenerator()
{
	if (nullptr != viewer_)
		delete viewer_;
}

PointCloud::Ptr PointCloudGenerator::generatePointCloud(Frame::Ptr frame)
{
    // 点云变量
    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud ( new PointCloud );
    // 遍历深度图
    for (int m = 0; m <  frame->depth_img_.rows; m++)
        for (int n=0; n < frame->depth_img_.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = frame->depth_img_.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
		continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / cam_->factor();
            p.x = (n - cam_->cx()) * p.z / cam_->fx();
            p.y = (m - cam_->cy()) * p.z / cam_->fy();

            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = frame->rgb_img_.ptr<uchar>(m)[n*3];
            p.g = frame->rgb_img_.ptr<uchar>(m)[n*3+1];
            p.r = frame->rgb_img_.ptr<uchar>(m)[n*3+2];

            // 把p加入到点云中
            cloud->points.push_back( p );
        }

	// 设置点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	// cout<<"point cloud size = "<<cloud->points.size()<<endl;
	cloud->is_dense = false;

// 	if (is_save_) {
// 		pcl::io::savePCDFile( "../data/pointcloud.pcd", *cloud );
// 		cout<<"Point cloud saved."<<endl;
// 	}

// 	if (is_show_) {
// 		viewer_->showCloud(cloud);
// 		// while ( ! viewer_->wasStopped()) { }
// 	}

	// 清除数据并退出
	// cloud->points.clear();

	return cloud;
}

int PointCloudGenerator::joinPointCloud(Frame::Ptr frame)
{
	// 转换成点云
	PointCloud::Ptr curr_cloud = generatePointCloud(frame);

	// 合并点云
	PointCloud::Ptr cloud_temp(new PointCloud);
	Eigen::Matrix3d rotation_matrix = frame->T_c2w_.rotation_matrix();
	Eigen::Vector3d translation_vector = frame->T_c2w_.translation();
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
	T.rotate(rotation_matrix);
	T.pretranslate(translation_vector);
	pcl::transformPointCloud(*curr_cloud, *cloud_temp, T.matrix());		// 将当前点云变换到世界坐标系下
	*cloud_temp += *cloud_out_;									// 在世界坐标系下将已有点云和当前帧点云进行拼接
	cloud_out_->clear();											// 清空旧的世界点云数据

	if (is_filter_) {
		// 拼接之后的点云进行体素滤波降采样
		voxel_.setInputCloud(cloud_temp);
		voxel_.filter(*cloud_out_);
	}
	else {
		cloud_out_ = cloud_temp;
	}

	// 可视化
	if (is_show_) {
		viewer_->showCloud(cloud_out_);
		// while ( ! viewer.wasStopped()) { }
	}

	return 0;
}

int PointCloudGenerator::savePointCloud()
{
	// 保存成pcd文件
	if (is_save_) {
		pcl::io::savePCDFile("../data/joinPointCloud.pcd", *cloud_out_);
	}

	return 0;
}


/* Copyright (C) 2017 IBM Corp.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/*********************************************************************
vec_l: A module for handling vectors of type long
**********************************************************************/
#include <NTL/matrix.h>
#include "mat_l.h"
#include "vec_l.h"

#include <NTL/new.h>
#include <NTL/vec_long.h>

NTL_CLIENT

//x = a-b
void sub(vec_l& x, const vec_l& a, const vec_l& b)
{
    long n = a.length();
    if (b.length() != n) LogicError("vector sub: dimension mismatch");
    x.SetLength(n);

    for (long i = 0; i < n; i++)
        x[i] = a[i] - b[i];

}

//res = x*a, naive implementation

void mul(long& res, const vec_l& x, const vec_l& a)
{
    long val;
    long n = a.length();
    long i;
    val = 0;
    for (i = 0; i < n; i++)
    {
        val += a.at(i)*x.at(i);
    }
    res = val;
}

//res = a-b
vec_l operator-(const vec_l& a, const vec_l& b)
{
    vec_l res;
    sub(res, a, b);
    return res;
    //NTL_OPT_RETURN(vec_l, res);
}

//res = a-b
vec_l operator-(const vec_l& a, const long& b)
{
    vec_l res;

    int n = a.length();
    int i;
    res.SetLength(n);
    for (i = 0; i< n; i++)
        //res.put(i,(a.get(i) - b));
        res[i] = a[i] - b;
    return res;
    //NTL_OPT_RETURN(vec_l, res);
}


// standard includes and external libraries
#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <ctime>
// project includes
#include "abstractclasses.h"
#include "bandit.h"
#include "oful.h"
#include "finitelinrep.h"
#include "utils.h"
#include "gzip.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

size_t PREC = 4;   // for saving numbers are rounded to PREC decimals
size_t EVERY = 1;  // save EVERY round

int main()
{

    int n_derank = 0;
    int low_rank = 15;

    const char *files[7] = { "../../problem_data/jester/33/1/jester_post_d33_span33.npz", "../../problem_data/jester/33/1/jester_post_d26_span26.npz", "../../problem_data/jester/33/1/jester_post_d24_span24.npz", "../../problem_data/jester/33/1/jester_post_d23_span23.npz", "../../problem_data/jester/33/1/jester_post_d20_span20.npz", "../../problem_data/jester/33/1/jester_post_d17_span17.npz", "../../problem_data/jester/33/1/jester_post_d16_span16.npz" };

    const char *names[9] = {"d=33", "d=26", "d=24", "d=23", "d=20", "d=17", "d=16", "d=16 (derank)", "d=17 (derank)"};

    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    int seed = time(NULL);
    srand (seed);
    cout << "seed: " << seed << endl;
    int n_runs = 50, T = 1000000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 1.0;
    double bonus_scale = 1.;
    bool adaptive_ci = true;

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    for (int j = 0; j < 7 + n_derank; j++) {

    // load reference representation

    auto start = TIC();
    // FiniteLinearRepresentation reference_rep=flr_loadjson("jester_post_d33_span33.json", noise_std, seed);
    FiniteLinearRepresentation reference_rep=flr_loadnpz("../../problem_data/jester/33/1/jester_post_d33_span33.npz", noise_std, seed,"features", "theta");
    auto tottime = TOC(start);
    int reference_rep_dim = reference_rep.features_dim();
    cout << "Loaded in " << tottime << endl;
    cout << "Ref_rep.dim: " << reference_rep_dim << endl;

    // load representation for OFUL
    start = TIC();
    // FiniteLinearRepresentation oful_rep = flr_loadjson("A.json", noise_std, seed);

    int l = j;
    if(j >= 7) {
	l = 7 + 6 - j;
    }
    FiniteLinearRepresentation oful_rep = flr_loadnpz(files[l], noise_std, seed, "features", "theta");

    if(j >= 7) {
        oful_rep = derank_hls(oful_rep, low_rank, false, true, true);
    }
    //oful_rep.normalize_features(10);

    tottime = TOC(start);
    int oful_rep_dim = oful_rep.features_dim();
    cout << "Loaded in " << tottime << endl;
    cout << "OFUL_rep.dim: " << oful_rep_dim << endl;

    cout << "Equal? " << reference_rep.is_equal(oful_rep, 1e-3) << endl;

    //just OFUL
    vec2double regrets, pseudo_regrets;

    #pragma omp parallel for
    for (int i = 0; i < n_runs; ++i)
    {
        OFUL<int> localg(oful_rep, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
        // create same representation but with different seed
        FiniteLinearRepresentation lrep = reference_rep.copy(seeds[i]);
        ContBanditProblem<int> prb(lrep, localg);
        prb.reset();
        auto start = TIC();
        prb.run(T);
        auto tottime = TOC(start);
        cout << "time(" << i << "): " << tottime << endl;

        // store regret and pseudo regret
        regrets.push_back(prb.instant_regret);
        pseudo_regrets.push_back(prb.exp_instant_regret);

        // save in compressed json
        save_vector_csv_gzip(regrets, "OFUL-"+std::string(names[j])+"_regrets.csv.gz", EVERY, PREC);
        save_vector_csv_gzip(pseudo_regrets, "OFUL-"+std::string(names[j])+"_pseudoregrets.csv.gz", EVERY, PREC);
    }
    }

    return 0;
}


/*
 * odeint_rk4_phase_lattice.cpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <cmath>

#include <boost/array.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>

#include "rk_performance_test_case.hpp"

#include "phase_lattice.hpp"

const size_t N = 1024;

typedef boost::array< double , N > state_type;
typedef boost::numeric::odeint::runge_kutta4_classic< state_type , double , state_type , double , boost::numeric::odeint::array_algebra> rk4_odeint_type;

class odeint_wrapper
{
public:
    void reset_init_cond()
    {
        for( size_t i = 0 ; i<N ; ++i )
            m_x[i] = 2.0*3.1415927*rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        m_stepper.do_step( phase_lattice<N>() , m_x , m_t , dt );
        //m_t += dt;
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
    rk4_odeint_type m_stepper;
};

int main()
{
    srand( 12312354 );

    odeint_wrapper stepper;

    run( stepper , 10000 , 1E-6 );
}


#ifndef OITK_CHESSBOARDSAMPLECONSENSUS_H
#define OITK_CHESSBOARDSAMPLECONSENSUS_H

#include <cmath>
#include <Eigen/Core>
#include <pcl/sample_consensus/sac_model.h>

namespace oitk
{
    inline void compressChessboardModel(Eigen::VectorXf &model,
        const Eigen::Vector3f &plane, const Eigen::Vector3f &origin, const Eigen::Vector3f &xdirection)
    {
        model.resize(6);
        model.head(3) = origin;
        // model.data()[3] = plane.data()[0] / plane.data()[2];
        model[3] = plane[0] / plane[2];
        model[4] = plane[1] / plane[2];

        Eigen::Vector3f absolute;
        absolute << 1, 0, 0;
        Eigen::Vector3f globalxdir = plane.cross(absolute);
        Eigen::Vector3f globalydir = plane.cross(globalxdir);

        model[5] = atan2f(xdirection.dot(globalydir) / globalydir.norm(),
                          xdirection.dot(globalxdir) / globalxdir.norm());
    }

    inline void decompressChessboardModel(const Eigen::VectorXf &model,
        Eigen::Vector3f &plane, Eigen::Vector3f &origin, Eigen::Vector3f &xdirection)
    {
        origin = model.head(3);
        plane[0] = model[3];
        plane[1] = model[4];
        plane[2] = 1;
        plane /= plane.norm();

        Eigen::Vector3f absolute;
        absolute << 1, 0, 0;
        Eigen::Vector3f globalxdir = plane.cross(absolute);
        Eigen::Vector3f globalydir = plane.cross(globalxdir);
        xdirection = globalxdir * cos(model[5]) + globalydir * sin(model[5]);
        xdirection /= xdirection.norm();
    }

    template <typename PointT>
    class SampleConsensusChessboard : public pcl::SampleConsensusModel<PointT>
    {
    public:
        typedef typename pcl::SampleConsensusModel<PointT>::PointCloud PointCloud;
        typedef typename pcl::SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
        typedef typename pcl::SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
        typedef boost::shared_ptr<SampleConsensusChessboard> Ptr;

    private:
        int _pattern_size;
        float _board_size, _edge_size, _rand_rate, _hole_size;
        Eigen::Vector3f _plane_coefficients;

        virtual bool isSampleGood(const std::vector<int> &samples) const override
        {
            if (samples.size() < 2)
                return false;

            // Get the values at the two points
            const Eigen::Map<const Eigen::Vector3f> p0 = input_->points[samples[0]].getVector3fMap();
            const Eigen::Map<const Eigen::Vector3f> p1 = input_->points[samples[1]].getVector3fMap();
            Eigen::Vector3f segment = (p1 - p0);

            constexpr float sq2 = 1.5; // a bit larger than sqrt(2)
            if (segment.norm() > _board_size * sq2)
                return false;

            return true;
        }

        inline float rndHalf()
        {
            constexpr int mod = 1024;
            return (rnd() % mod - (mod >> 1)) / (float)mod;
        }

        inline float distanceToHoleEdge(const float &relative_coord)
        {
            // XXX: Assert relative_coord is positive
            float times = relative_coord / (_hole_size * 2)
                + ((_pattern_size / 2 + 1) % 2 == 0 ? 0.25 : -0.25);
            times = times - (int)times;
            if (times < 0.25)
                return times * _hole_size * 2;
            else if (times < 0.5)
                return (0.5 - times) * _hole_size * 2;
            else return 0;
        }

        inline float distanceToBoard(const float &board_x, const float &board_y)
        {
            // XXX: Assert board_x and board_y is positive
            float outer_edge = _board_size / 2;
            float dx = board_x - outer_edge;
            float dy = board_y - outer_edge;
            if (dx > 0)
            {
                if (dy > 0)
                    return sqrt(dx * dx + dy * dy);
                else
                    return dx;
            }
            else if (board_y > outer_edge)
                return dy;
            else
                return std::min(distanceToHoleEdge(board_x),
                           distanceToHoleEdge(board_y));
        }

    protected:
        using pcl::SampleConsensusModel<PointT>::model_name_;
        using pcl::SampleConsensusModel<PointT>::input_;
        using pcl::SampleConsensusModel<PointT>::indices_;
        using pcl::SampleConsensusModel<PointT>::error_sqr_dists_;
        using pcl::SampleConsensusModel<PointT>::sample_size_;
        using pcl::SampleConsensusModel<PointT>::model_size_;
        using pcl::SampleConsensusModel<PointT>::rnd;

    private:
        /** \brief Functor for the optimization function */
        struct OptimizationFunctor : pcl::Functor<float>
        {
            OptimizationFunctor(int n_data_points, SampleConsensusChessboard<PointT> *model) :
                pcl::Functor<float>(n_data_points), model_(model) {}

            int operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
            {
                // TODO: Implement
            }

            SampleConsensusChessboard<PointT> *model_;
        };

    public:
        SampleConsensusChessboard(const PointCloudConstPtr &cloud,
            int pattern_size, float board_size, float edge_size,
            Eigen::VectorXf plane_coeffs, bool random = false)
            : pcl::SampleConsensusModel<PointT>(cloud, random),
            _pattern_size(pattern_size), _board_size(board_size), _edge_size(edge_size),
            _hole_size((board_size - 2 * edge_size) / (pattern_size - 1)), _rand_rate(1)
        {
            model_name_ = "SampleConsensusChessboard";
            sample_size_ = 2;
            model_size_ = 6;

            if (plane_coeffs.size() < 3)
                cerr << "Wrong plane coefficients input. Please estimate the plane before finding the board!" << endl;
            for (int i = 0; i < 3; i++)
                _plane_coefficients[i] = plane_coeffs[i];
            _plane_coefficients /= _plane_coefficients.norm();
        }

        SampleConsensusChessboard(const PointCloudConstPtr &cloud, const std::vector<int> &indices,
            int pattern_size, float board_size, float edge_size,
            Eigen::VectorXf plane_coeffs, bool random = false)
            : pcl::SampleConsensusModel<PointT>(cloud, indices, random),
            _pattern_size(pattern_size), _board_size(board_size), _edge_size(edge_size),
            _hole_size((board_size - 2 * edge_size) / (pattern_size - 1)), _rand_rate(1)
        {
            model_name_ = "SampleConsensusChessboard";
            sample_size_ = 2;
            model_size_ = 6;

            if (plane_coeffs.size() < 3)
                cerr << "Wrong plane coefficients input. Please estimate the plane before finding the board!" << endl;
            for (int i = 0; i < 3; i++)
                _plane_coefficients[i] = plane_coeffs[i];
            _plane_coefficients /= _plane_coefficients.norm();
        }

        virtual bool computeModelCoefficients(const std::vector<int> &samples,
            Eigen::VectorXf &model_coefficients) override
        {
            pcl::Array3fMapConst p0 = input_->points[samples[0]].getArray3fMap();
            pcl::Array3fMapConst p1 = input_->points[samples[1]].getArray3fMap();

            Eigen::Array3f origin = (p0 + p1) / 2;
            origin += (p0 - origin) * rndHalf() * _rand_rate;
            Eigen::Vector3f diameter = p1 - p0;
            Eigen::Vector3f adiameter = _plane_coefficients.cross(diameter);

            Eigen::Vector3f xdirection = diameter / diameter.norm() * rndHalf() * _rand_rate
                                       + adiameter / adiameter.norm() * rndHalf() * _rand_rate;
            xdirection /= xdirection.norm();

            compressChessboardModel(model_coefficients, _plane_coefficients, origin, xdirection);
            return true;
        }

        virtual void optimizeModelCoefficients(const std::vector<int> &inliers,
            const Eigen::VectorXf &model_coefficients,
            Eigen::VectorXf &optimized_coefficients) override
        {
            // TODO: Implement
            cerr << "Not Implemented yet" << endl;
        }

        virtual void getDistancesToModel(const Eigen::VectorXf &model_coefficients,
            std::vector<double> &distances) override
        {
            distances.resize(indices_->size());

            Eigen::Vector3f plane_normal, origin, xdirection, ydirection;
            decompressChessboardModel(model_coefficients, plane_normal, origin, xdirection);
            ydirection = plane_normal.cross(xdirection);

            for (int idx = 0; idx < indices_->size(); idx++)
            {
                // Project to plane
                Eigen::Vector3f direct_dist =
                    input_->points[(*indices_)[idx]].getArray3fMap() - origin.array();
                float zdist = direct_dist.dot(plane_normal);
                direct_dist -= zdist * plane_normal;
                float board_x = abs(direct_dist.dot(xdirection));
                float board_y = abs(direct_dist.dot(ydirection));

                // Calculate distance
                float bdist = distanceToBoard(board_x, board_y);
                // distances[idx] = sqrt(bdist * bdist + zdist * zdist);
                distances[idx] = bdist; // Just consider the distance in board
            }
        }

        virtual void selectWithinDistance(const Eigen::VectorXf &model_coefficients,
            const double threshold, std::vector<int> &inliers) override
        {
            std::vector<double> distances;
            getDistancesToModel(model_coefficients, distances);

            int counter = 0;
            inliers.resize(indices_->size());
            error_sqr_dists_.resize(indices_->size());
            for (int idx = 0;idx < distances.size(); idx++)
            {
                if (distances[idx] >= threshold) continue;
                inliers[counter] = (*indices_)[idx];
                error_sqr_dists_[counter] = distances[idx];
                counter++;
            }
            inliers.resize(counter);
            error_sqr_dists_.resize(counter);
        }

        virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients,
            const double threshold) override
        {
            std::vector<double> distances;
            getDistancesToModel(model_coefficients, distances);

            int counter = 0;
            for (double distance : distances)
                if (distance < threshold) counter++;
            return counter;
        }

        virtual void projectPoints(const std::vector<int> &inliers,
            const Eigen::VectorXf &model_coefficients,
            PointCloud &projected_points,
            bool copy_data_fields = true) override
        {
            // TODO: Implement
            cerr << "Not Implemented yet" << endl;
        }

        virtual bool doSamplesVerifyModel(const std::set<int> &indices,
            const Eigen::VectorXf &model_coefficients,
            const double threshold) override
        {
            // TODO: Implement
            cerr << "Not Implemented yet" << endl;
            return false;
        }

        virtual pcl::SacModel getModelType() const override
        {
            cerr << "SampleConsensusChessboard is not a sample consensus type built in pcl!" << endl;
            return pcl::SACMODEL_PLANE;
        }

        /** \brief Set the randomization range of the board location
          * \param[in] rate the randomization range (0~1)
          */
        inline void setRandomRate(const float &rate)
        {
            _rand_rate = rate;
        }

        /** \brief Get the randomization range of the board location
          * \param[out] rate the randomization range (0~1)
          */
        inline void getRandomRate(float &rate)
        {
            rate = _rand_rate;
        }
    };
}

#endif // OITK_CHESSBOARDSAMPLECONSENSUS_H


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

#ifndef BOOST_TR1_CMATH_HPP_INCLUDED
#  define BOOST_TR1_CMATH_HPP_INCLUDED
#  include <boost/tr1/detail/config.hpp>

#ifdef BOOST_HAS_TR1_CMATH

#  if defined(BOOST_HAS_INCLUDE_NEXT) && !defined(BOOST_TR1_DISABLE_INCLUDE_NEXT)
#     include_next BOOST_TR1_HEADER(cmath)
#  else
#     include <boost/tr1/detail/config_all.hpp>
#     include BOOST_TR1_HEADER(cmath)
#  endif

#else

#include <boost/math/tr1.hpp>

namespace std{ namespace tr1{

using boost::math::tr1::assoc_laguerre;
using boost::math::tr1::assoc_laguerref;
using boost::math::tr1::assoc_laguerrel;
// [5.2.1.2] associated Legendre functions:
using boost::math::tr1::assoc_legendre;
using boost::math::tr1::assoc_legendref;
using boost::math::tr1::assoc_legendrel;
// [5.2.1.3] beta function:
using boost::math::tr1::beta;
using boost::math::tr1::betaf;
using boost::math::tr1::betal;
// [5.2.1.4] (complete) elliptic integral of the first kind:
using boost::math::tr1::comp_ellint_1;
using boost::math::tr1::comp_ellint_1f;
using boost::math::tr1::comp_ellint_1l;
// [5.2.1.5] (complete) elliptic integral of the second kind:
using boost::math::tr1::comp_ellint_2;
using boost::math::tr1::comp_ellint_2f;
using boost::math::tr1::comp_ellint_2l;
// [5.2.1.6] (complete) elliptic integral of the third kind:
using boost::math::tr1::comp_ellint_3;
using boost::math::tr1::comp_ellint_3f;
using boost::math::tr1::comp_ellint_3l;
#if 0
// [5.2.1.7] confluent hypergeometric functions:
using boost::math::tr1::conf_hyperg;
using boost::math::tr1::conf_hypergf;
using boost::math::tr1::conf_hypergl;
#endif
// [5.2.1.8] regular modified cylindrical Bessel functions:
using boost::math::tr1::cyl_bessel_i;
using boost::math::tr1::cyl_bessel_if;
using boost::math::tr1::cyl_bessel_il;
// [5.2.1.9] cylindrical Bessel functions (of the first kind):
using boost::math::tr1::cyl_bessel_j;
using boost::math::tr1::cyl_bessel_jf;
using boost::math::tr1::cyl_bessel_jl;
// [5.2.1.10] irregular modified cylindrical Bessel functions:
using boost::math::tr1::cyl_bessel_k;
using boost::math::tr1::cyl_bessel_kf;
using boost::math::tr1::cyl_bessel_kl;
// [5.2.1.11] cylindrical Neumann functions;
// cylindrical Bessel functions (of the second kind):
using boost::math::tr1::cyl_neumann;
using boost::math::tr1::cyl_neumannf;
using boost::math::tr1::cyl_neumannl;
// [5.2.1.12] (incomplete) elliptic integral of the first kind:
using boost::math::tr1::ellint_1;
using boost::math::tr1::ellint_1f;
using boost::math::tr1::ellint_1l;
// [5.2.1.13] (incomplete) elliptic integral of the second kind:
using boost::math::tr1::ellint_2;
using boost::math::tr1::ellint_2f;
using boost::math::tr1::ellint_2l;
// [5.2.1.14] (incomplete) elliptic integral of the third kind:
using boost::math::tr1::ellint_3;
using boost::math::tr1::ellint_3f;
using boost::math::tr1::ellint_3l;
// [5.2.1.15] exponential integral:
using boost::math::tr1::expint;
using boost::math::tr1::expintf;
using boost::math::tr1::expintl;
// [5.2.1.16] Hermite polynomials:
using boost::math::tr1::hermite;
using boost::math::tr1::hermitef;
using boost::math::tr1::hermitel;
#if 0
// [5.2.1.17] hypergeometric functions:
using boost::math::tr1::hyperg;
using boost::math::tr1::hypergf;
using boost::math::tr1::hypergl;
#endif
// [5.2.1.18] Laguerre polynomials:
using boost::math::tr1::laguerre;
using boost::math::tr1::laguerref;
using boost::math::tr1::laguerrel;
// [5.2.1.19] Legendre polynomials:
using boost::math::tr1::legendre;
using boost::math::tr1::legendref;
using boost::math::tr1::legendrel;
// [5.2.1.20] Riemann zeta function:
using boost::math::tr1::riemann_zeta;
using boost::math::tr1::riemann_zetaf;
using boost::math::tr1::riemann_zetal;
// [5.2.1.21] spherical Bessel functions (of the first kind):
using boost::math::tr1::sph_bessel;
using boost::math::tr1::sph_besself;
using boost::math::tr1::sph_bessell;
// [5.2.1.22] spherical associated Legendre functions:
using boost::math::tr1::sph_legendre;
using boost::math::tr1::sph_legendref;
using boost::math::tr1::sph_legendrel;
// [5.2.1.23] spherical Neumann functions;
// spherical Bessel functions (of the second kind):
using boost::math::tr1::sph_neumann;
using boost::math::tr1::sph_neumannf;
using boost::math::tr1::sph_neumannl;

// types
using boost::math::tr1::double_t;
using boost::math::tr1::float_t;
// functions
using boost::math::tr1::acosh;
using boost::math::tr1::acoshf;
using boost::math::tr1::acoshl;
using boost::math::tr1::asinh;
using boost::math::tr1::asinhf;
using boost::math::tr1::asinhl;
using boost::math::tr1::atanh;
using boost::math::tr1::atanhf;
using boost::math::tr1::atanhl;
using boost::math::tr1::cbrt;
using boost::math::tr1::cbrtf;
using boost::math::tr1::cbrtl;
using boost::math::tr1::copysign;
using boost::math::tr1::copysignf;
using boost::math::tr1::copysignl;
using boost::math::tr1::erf;
using boost::math::tr1::erff;
using boost::math::tr1::erfl;
using boost::math::tr1::erfc;
using boost::math::tr1::erfcf;
using boost::math::tr1::erfcl;
#if 0
using boost::math::tr1::exp2;
using boost::math::tr1::exp2f;
using boost::math::tr1::exp2l;
#endif
using boost::math::tr1::expm1;
using boost::math::tr1::expm1f;
using boost::math::tr1::expm1l;
#if 0
using boost::math::tr1::fdim;
using boost::math::tr1::fdimf;
using boost::math::tr1::fdiml;
using boost::math::tr1::fma;
using boost::math::tr1::fmaf;
using boost::math::tr1::fmal;
#endif
using boost::math::tr1::fmax;
using boost::math::tr1::fmaxf;
using boost::math::tr1::fmaxl;
using boost::math::tr1::fmin;
using boost::math::tr1::fminf;
using boost::math::tr1::fminl;
using boost::math::tr1::hypot;
using boost::math::tr1::hypotf;
using boost::math::tr1::hypotl;
#if 0
using boost::math::tr1::ilogb;
using boost::math::tr1::ilogbf;
using boost::math::tr1::ilogbl;
#endif
using boost::math::tr1::lgamma;
using boost::math::tr1::lgammaf;
using boost::math::tr1::lgammal;
#if 0
using boost::math::tr1::llrint;
using boost::math::tr1::llrintf;
using boost::math::tr1::llrintl;
#endif
using boost::math::tr1::llround;
using boost::math::tr1::llroundf;
using boost::math::tr1::llroundl;
using boost::math::tr1::log1p;
using boost::math::tr1::log1pf;
using boost::math::tr1::log1pl;
#if 0
using boost::math::tr1::log2;
using boost::math::tr1::log2f;
using boost::math::tr1::log2l;
using boost::math::tr1::logb;
using boost::math::tr1::logbf;
using boost::math::tr1::logbl;
using boost::math::tr1::lrint;
using boost::math::tr1::lrintf;
using boost::math::tr1::lrintl;
#endif
using boost::math::tr1::lround;
using boost::math::tr1::lroundf;
using boost::math::tr1::lroundl;
#if 0
using boost::math::tr1::nan;
using boost::math::tr1::nanf;
using boost::math::tr1::nanl;
using boost::math::tr1::nearbyint;
using boost::math::tr1::nearbyintf;
using boost::math::tr1::nearbyintl;
#endif
using boost::math::tr1::nextafter;
using boost::math::tr1::nextafterf;
using boost::math::tr1::nextafterl;
using boost::math::tr1::nexttoward;
using boost::math::tr1::nexttowardf;
using boost::math::tr1::nexttowardl;
#if 0
using boost::math::tr1::remainder;
using boost::math::tr1::remainderf;
using boost::math::tr1::remainderl;
using boost::math::tr1::remquo;
using boost::math::tr1::remquof;
using boost::math::tr1::remquol;
using boost::math::tr1::rint;
using boost::math::tr1::rintf;
using boost::math::tr1::rintl;
#endif
using boost::math::tr1::round;
using boost::math::tr1::roundf;
using boost::math::tr1::roundl;
#if 0
using boost::math::tr1::scalbln;
using boost::math::tr1::scalblnf;
using boost::math::tr1::scalblnl;
using boost::math::tr1::scalbn;
using boost::math::tr1::scalbnf;
using boost::math::tr1::scalbnl;
#endif
using boost::math::tr1::tgamma;
using boost::math::tr1::tgammaf;
using boost::math::tr1::tgammal;
using boost::math::tr1::trunc;
using boost::math::tr1::truncf;
using boost::math::tr1::truncl;
// C99 macros defined as C++ templates
using boost::math::tr1::signbit;
using boost::math::tr1::fpclassify;
using boost::math::tr1::isfinite;
using boost::math::tr1::isinf;
using boost::math::tr1::isnan;
using boost::math::tr1::isnormal;
#if 0
using boost::math::tr1::isgreater;
using boost::math::tr1::isgreaterequal;
using boost::math::tr1::isless;
using boost::math::tr1::islessequal;
using boost::math::tr1::islessgreater;
using boost::math::tr1::isunordered;
#endif
} } // namespaces

#endif // BOOST_HAS_TR1_CMATH

#endif // BOOST_TR1_CMATH_HPP_INCLUDED


#pragma once

#include "ArithmeticProgression.hpp"
#include "Misc.hpp"
#include "Reversed.hpp"
#include "Sequences.hpp"
#include "VectorHelpers.hpp"
#include "detail/PartitionsDetail.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

////////////////////////////////////////////////////////////
/// \brief class of partitions of the number n.
/// \param IntType should be an integral type with enough space to store n and
/// k. It can be signed or unsigned. # Example:
///
///	 partitions X(6);
///		for (auto&& x : X)
///			cout << x << ' ';
///
/// Prints out:
///
/// 	[ 1 1 1 1 1 1 ] [ 2 1 1 1 1 ] [ 3 1 1 1 ] [ 2 2 1 1 ] [ 4 1 1 ] [ 3 2 1
/// ] [ 2 2 2 ] [ 5 1 ] [ 4 2 ] [ 3 3 ] [ 6 ]
////////////////////////////////////////////////////////////
template <class IntType = int, class RAContainerInt = std::vector<IntType>>
class Partitions
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using value_type = RAContainerInt;
    using partition = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;
    class reverse_iterator;
    using const_reverse_iterator = reverse_iterator;

    ////////////////////////////////////////////////////////////
    /// \brief Constructor
    ///
    /// \param n is an integer >= 0
    ///
    ////////////////////////////////////////////////////////////
    explicit Partitions(IntType n)
        : n_(n), min_num_parts_(1), max_num_parts_(n), size_(calc_size(n))
    {}

    ////////////////////////////////////////////////////////////
    /// \brief Constructor
    ///
    /// \param n is an integer >= 0
    /// \param numparts is an integer >= 1 and <= n
    ///
    ////////////////////////////////////////////////////////////
    Partitions(IntType n, IntType numparts)
        : n_(n)
        , min_num_parts_(numparts)
        , max_num_parts_(numparts)
        , size_(calc_size(n, numparts))
    {}

    ////////////////////////////////////////////////////////////
    /// \brief Constructor
    ///
    /// \param n is an integer >= 0
    /// \param minnumparts is an integer >= 1 and <= n
    /// \param maxnumparts is an integer >= minnumparts and <= n
    ///
    ////////////////////////////////////////////////////////////
    Partitions(IntType n, IntType minnumparts, IntType maxnumparts)
        : n_(n)
        , min_num_parts_(minnumparts)
        , max_num_parts_(maxnumparts)
        , size_(calc_size(n, minnumparts, maxnumparts))
    {}

    ////////////////////////////////////////////////////////////
    /// \brief The total number of partitions
    ///
    /// \return p_n
    ///
    ////////////////////////////////////////////////////////////
    size_type size() const { return size_; }

    IntType get_n() const { return n_; }

    iterator begin() const { return iterator(n_, max_num_parts_); }

    const iterator end() const
    {
        return iterator::make_invalid_with_id(size());
    }

    reverse_iterator rbegin() const
    {
        return reverse_iterator(n_, min_num_parts_);
    }

    const reverse_iterator rend() const
    {
        return reverse_iterator::make_invalid_with_id(size());
    }

    template <class Func>
    void for_each(Func f) const
    {
        for (auto k : reversed(II(min_num_parts_, max_num_parts_ + 1)))
        {
            for_each(f, k);
        }
    }

    ////////////////////////////////////////////////////////////
    /// \brief Bidirectional iterator class.
    ////////////////////////////////////////////////////////////
    class iterator
        : public boost::iterator_facade<iterator, const partition&, boost::bidirectional_traversal_tag>
    {
    public:
        iterator() : n_(0), data_() {}

        explicit iterator(IntType n, IntType numparts)
            : n_(n), data_(numparts, 1)
        {
            if (numparts > 0)
                data_[0] = n - numparts + 1;
        }

        inline size_type ID() const { return ID_; }

        // boost::iterator_facade provides all the public interface you need,
        // like ++, etc.

        static const iterator make_invalid_with_id(size_type id)
        {
            iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;

            next_partition(data_, n_);
        }

        void decrement()
        {
            --ID_;

            prev_partition(data_, n_);
        }

        const partition& dereference() const { return data_; }

        bool equal(const iterator& it) const { return it.ID() == ID(); }

        difference_type distance_to(const iterator& lhs) const
        {
            return static_cast<difference_type>(lhs.ID()) - ID();
        }

    private:
        size_type ID_{0};
        IntType n_;
        partition data_;

        friend class boost::iterator_core_access;
    }; // end class iterator

    ////////////////////////////////////////////////////////////
    /// \brief Bidirectional iterator class.
    ////////////////////////////////////////////////////////////
    class reverse_iterator
        : public boost::iterator_facade<reverse_iterator,
                                        const partition&,
                                        boost::bidirectional_traversal_tag>
    {
    public:
        reverse_iterator() : n_(0), data_() {}

        explicit reverse_iterator(IntType n, IntType numparts) : n_(n), data_()
        {
            last_with_given_number_of_parts(data_, n, numparts);
        }

        inline size_type ID() const { return ID_; }

        // boost::iterator_facade provides all the public interface you need,
        // like ++, etc.

        static const reverse_iterator make_invalid_with_id(size_type id)
        {
            reverse_iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;

            prev_partition(data_, n_);
        }

        void decrement()
        {
            --ID_;

            next_partition(data_, n_);
        }

        const partition& dereference() const { return data_; }

        bool equal(const reverse_iterator& it) const { return it.ID() == ID(); }

        difference_type distance_to(const reverse_iterator& lhs) const
        {
            return static_cast<difference_type>(lhs.ID()) - ID();
        }

    private:
        size_type ID_{0};
        IntType n_;
        partition data_;

        friend class boost::iterator_core_access;
    }; // end class reverse_iterator

    // **************** Begin static functions
    static void next_partition(partition& data, IntType n)
    {
        size_t t = data.size();

        if (t < 2)
        {
            return;
        }

        if (data.front() - data.back() < 2) // We must change size!
        {
            first_with_given_number_of_parts(data, n, t - 1);
            return;
        }

        // If no size change is necessary

        // Starting from the end, we look at the first whose difference is at
        // least 2 in order to transfer one unit from that one and then divide
        // unevenly among the other ones.
        IntType smallest = data.back();
        difference_type suffixSum = smallest;

        for (difference_type i = t - 2; i >= 0; --i)
        {
            if (data[i] - smallest > 1)
            {
                --data[i];
                distribute_unevenly(data.begin() + i + 1,
                                    data.end(),
                                    suffixSum + 1,
                                    data[i]);
                return;
            }
            suffixSum += data[i];
        }
    }

    static void prev_partition(partition& data, IntType n)
    {
        size_type t = data.size();
        if (t == 0)
            return;
        if (t == 1 || data[1] == 1)
        {
            last_with_given_number_of_parts(data, n, t + 1);
            return;
        }

        difference_type suffixSum = data.back();

        for (IntType i = t - 2; i >= 0; --i)
        {

            if (can_increase(data, i))
            {
                ++data[i];
                distribute_evenly(data.begin() + i + 1, data.end(), suffixSum - 1);
                return;
            }
            suffixSum += data[i];
        }
    }

    static void first_with_given_number_of_parts(partition& data,
                                                 IntType n,
                                                 IntType k)
    {
        if (n == 0)
        {
            data.clear();
            return;
        }

        data.resize(k);

        std::fill(data.begin(), data.end(), 1);

        data[0] = n - k + 1;
    }

    static void last_with_given_number_of_parts(partition& data,
                                                IntType n,
                                                IntType k)
    {
        if (n == 0)
        {
            data.clear();
            return;
        }
        data.resize(k);

        distribute_evenly(data.begin(), data.end(), n);
    }

    static partition conjugate(const partition& P)
    {
        assert(!P.empty());
        partition result(P[0], 1);
        auto n = P.size();

        result[0] = n;

        for (size_t i = 1; i < n; ++i)
        {
            auto t =
              std::lower_bound(P.begin(), P.end(), i, std::greater<IntType>());

            int r = t - P.begin();

            if (r > 0)
                result[i] = r;
        }

        return result;
    }

    // **************** End static functions

private:
    IntType n_;
    IntType min_num_parts_;
    IntType max_num_parts_;
    size_type size_;

    static size_type calc_size(IntType n) { return partition_number(n); }

    static size_type calc_size(IntType n, IntType numparts)
    {
        return partition_number(n, numparts);
    }

    static size_type calc_size(IntType n, IntType minnumparts, IntType maxnumparts)
    {
        size_type toReturn = 0;
        for (size_type k = minnumparts; k <= maxnumparts; ++k)
            toReturn += partition_number(n, k);
        return toReturn;
    }

    static bool can_increase(const partition& data, size_type i)
    {
        if (i == 0)
            return true;

        if (data[i] == 1 || data[i + 1] == 1)
            return false;

        if (data[i - 1] == data[i])
            return false;

        return true;
    }

    template <class Iter>
    static void distribute_evenly(const Iter& first, const Iter& last, IntType n)
    {
        if (first == last)
            return;
        IntType k = last - first;

        auto quot_rem = std::div(n, k);

        //         IntType lower = n/k;
        //         IntType residue = n - lower*k;

        auto mid = first + quot_rem.rem;
        std::fill(first, mid, quot_rem.quot + 1);
        std::fill(mid, last, quot_rem.quot);
    }

    template <class Iter>
    static void distribute_unevenly(Iter first,
                                    const Iter& last,
                                    IntType n,
                                    IntType maximum)
    {
        auto k = last - first;
        IntType excess = n - k;
        for (; first != last; ++first)
        {
            *first = std::min<IntType>(maximum, excess + 1);
            excess += (1 - *first);
        }
    }

    template <class Func>
    void for_each(Func f, IntType k) const
    {
        // I'm really sorry about this. I don't know how to improve it. If you
        // do, by all means, tell me about it.
        switch (k)
        {
            // clang-format off
        using part = partition;
        case 0: detail::for_each_partition<part, 0>::apply(n_, f); break;
        case 1: detail::for_each_partition<part, 1>::apply(n_, f); break;
        case 2: detail::for_each_partition<part, 2>::apply(n_, f); break;
        case 3: detail::for_each_partition<part, 3>::apply(n_, f); break;
        case 4: detail::for_each_partition<part, 4>::apply(n_, f); break;
        case 5: detail::for_each_partition<part, 5>::apply(n_, f); break;
        case 6: detail::for_each_partition<part, 6>::apply(n_, f); break;
        case 7: detail::for_each_partition<part, 7>::apply(n_, f); break;
        case 8: detail::for_each_partition<part, 8>::apply(n_, f); break;
        case 9: detail::for_each_partition<part, 9>::apply(n_, f); break;
        case 10: detail::for_each_partition<part, 10>::apply(n_, f); break;
        case 11: detail::for_each_partition<part, 11>::apply(n_, f); break;
        case 12: detail::for_each_partition<part, 12>::apply(n_, f); break;
        case 13: detail::for_each_partition<part, 13>::apply(n_, f); break;
        case 14: detail::for_each_partition<part, 14>::apply(n_, f); break;
        case 15: detail::for_each_partition<part, 15>::apply(n_, f); break;
        case 16: detail::for_each_partition<part, 16>::apply(n_, f); break;

            // clang-format on

        default:
        {
            for (auto&& x : Partitions<IntType, RAContainerInt>(n_, k))
            {
                f(x);
            }

            break;
        }
        } // end switch(k)
    }

}; // end class Partitions

using boost::container::static_vector;

using partitions = Partitions<int>;
using partitions_stack = Partitions<int, static_vector<int, 128>>;

} // namespace discreture


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

#include "frequency.hpp"
#include "interval_type.hpp"
#include "dcm_exceptions.hpp"
#include "string_conv.hpp"
#include <boost/lexical_cast.hpp>
#include <jewel/decimal.hpp>
#include <jewel/decimal_exceptions.hpp>
#include <jewel/exception.hpp>
#include <wx/string.h>
#include <string>

using boost::lexical_cast;
using jewel::Decimal;
using jewel::DecimalMultiplicationException;
using std::string;

namespace dcm
{

namespace
{
    Decimal const& days_per_year()
    {
        static Decimal const ret("365.25");
        return ret;
    }
    Decimal const& days_per_week()
    {
        static Decimal const ret("7");
        return ret;
    }
    Decimal const& months_per_year()
    {
        static Decimal const ret("12");
        return ret;
    }
    Decimal const& days_per_canonical_interval()
    {
        // A number with that's wholly divisible
        // by various commonly used Frequencies expressed
        // in numbers of days.
        static Decimal const ret(654528, 0);
        return ret;
    }
    Decimal const& weeks_per_canonical_interval()
    {
        static Decimal const ret =
            days_per_canonical_interval() / days_per_week();
        JEWEL_ASSERT (round(ret, 0) == ret);
        return ret;
    }
    Decimal const& years_per_canonical_interval()
    {
        static Decimal const ret =
            days_per_canonical_interval() / days_per_year();
        JEWEL_ASSERT (round(ret, 0) == ret);
        return ret;
    }
    Decimal const& months_per_canonical_interval()
    {
        static Decimal const ret =
            years_per_canonical_interval() * months_per_year();
        JEWEL_ASSERT (round(ret, 0) == ret);
        return ret;
    }

}  // end anonymous namespace

Frequency::Frequency
(   int p_num_steps,
    IntervalType p_step_type
):
    m_num_steps(p_num_steps),
    m_step_type(p_step_type)
{
    if (p_num_steps < 1)
    {
        JEWEL_THROW
        (   InvalidFrequencyException,
            "In Frequency constructor, p_num_steps passed a value less than 1."
        );
    }
    JEWEL_ASSERT (p_num_steps > 0);
}

int
Frequency::num_steps() const
{
    return m_num_steps;
}

IntervalType
Frequency::step_type() const
{
    return m_step_type;
}

string
frequency_description(Frequency const& frequency, string const& first_word)
{
    string ret = first_word + " ";
    int const num_steps = frequency.num_steps();
    if (num_steps > 1)
    {
        ret += lexical_cast<string>(num_steps);
        ret += " ";
        ret += wx_to_std8(phrase(frequency.step_type(), true));
    }
    else
    {
        ret += wx_to_std8(phrase(frequency.step_type(), false));
    }
    return ret;
}

Frequency const&
canonical_frequency()
{
    JEWEL_ASSERT
    (   round(days_per_canonical_interval(), 0) ==
        days_per_canonical_interval()
    );
    static Frequency const ret
    (   round(days_per_canonical_interval(), 0).intval(),
        IntervalType::days
    );
    return ret;
}

Decimal
convert_to_canonical(Frequency const& p_frequency, Decimal const& p_amount)
{
    auto const num_steps = p_frequency.num_steps();
    static_assert
    (   sizeof(Decimal::int_type) >= sizeof(num_steps),
        "Potentially unsafe integral conversion."
    );
    Decimal const steps(num_steps, 0);  // will not throw

    // The next part could throw DecimalMultiplicationException or
    // DecimalDivisionException.
    switch (p_frequency.step_type())
    {
    case IntervalType::days:
        return p_amount * days_per_canonical_interval() / steps;
    case IntervalType::weeks:
        return p_amount * weeks_per_canonical_interval() / steps;
    case IntervalType::months:  // fall through
    case IntervalType::month_ends:
        return p_amount * months_per_canonical_interval() / steps;
    default:
        JEWEL_HARD_ASSERT (false);
    }
}

Decimal
convert_from_canonical(Frequency const& p_frequency, Decimal const& p_amount)
{
    auto const num_steps = p_frequency.num_steps();
    static_assert
    (   sizeof(Decimal::int_type) >= sizeof(num_steps),
        "Potentially unsafe integral conversion."
    );
    Decimal const steps(num_steps, 0);  // will not throw

    // Might throw DecimalMultiplicationException.
    Decimal const intermediate = p_amount * steps;

    // The next part will throw jewel::DecimalDivisionException if and only if
    // the number of significant digits in intermediate is
    // equal to jewel::Decimal::maximum_precision().
    switch (p_frequency.step_type())
    {
    case IntervalType::days:
        return intermediate / days_per_canonical_interval();
    case IntervalType::weeks:
        return intermediate / weeks_per_canonical_interval();
    case IntervalType::months:  // fall through
    case IntervalType::month_ends:
        return intermediate / months_per_canonical_interval();
    default:
        JEWEL_HARD_ASSERT (false);
    }
}

bool
operator==(Frequency const& lhs, Frequency const& rhs)
{
    return
        (lhs.num_steps() == rhs.num_steps()) &&
        (lhs.step_type() == rhs.step_type());
}

bool
operator!=(Frequency const& lhs, Frequency const& rhs)
{
    return !(lhs == rhs);
}

}  // namespace dcm