#include <iostream>
#include <memory>
#include <vector>
#include <string>
#include <unordered_map>
#include <Eigen/Dense>
#include "../include/trainer.h"
#include "../include/model.h"
#include "../include/dataset.h"
#include "../include/optimizer.h"

namespace MyDL{

    using namespace Eigen;
    using std::cout;
    using std::endl;
    using std::vector;
    using std::string;
    using std::unordered_map;
    using std::shared_ptr;

    Trainer::Trainer(shared_ptr<BaseModel> model, shared_ptr<Optimizer> optimizer, shared_ptr<Dataset> dataset, int epochs, bool verbose)
    {
        _model = model;
        _optimizer = optimizer;
        _dataset = dataset;
        _epochs = epochs;
        _verbose = verbose;
        _train_size = _dataset->get_train_size();
        _batch_size = _dataset->get_batch_size();
        _iter_per_epoch = (_train_size + _batch_size - 1) / _batch_size; // 切り上げ
        _max_iter = _epochs * _iter_per_epoch;

    }

    void Trainer::train_step(void)
    {
        vector<MatrixXd> inputs, loss, val_inputs;
        unordered_map<string, MatrixXd> grads;
        unordered_map<string, shared_ptr<MatrixXd>> params;
        MatrixXd train_X, train_y, test_X, test_y;

        _dataset->next_train(train_X, train_y);

        inputs.push_back(train_X);

        grads = _model->gradient(inputs, train_y);
        params = _model->get_params();

        _optimizer->update(params, grads);

        loss = _model->loss(inputs, train_y);
        train_loss_history.push_back(double(loss[0](0)));

        if (_verbose)
        {
            // デバッグ用(verboseオプションで表示するようにしても良いかも)
            cout << "Train loss: " << loss[0] << endl;
        }

        if (_current_iter % _iter_per_epoch == 0)
        {
            _current_epoch += 1;

            // accuracyの確認(内部的にエポックごとに確認する機構を設ける)
            _dataset->next_test(test_X, test_y);

            val_inputs.push_back(test_X);

            double train_acc, test_acc;
            train_acc = _model->accuracy(inputs, train_y);
            test_acc = _model->accuracy(val_inputs, test_y);

            train_acc_history.push_back(train_acc);
            test_acc_history.push_back(test_acc);

            if (_verbose)
            {
                cout << "=== epoch: " << _current_epoch << ", train acc: " << train_acc << ", test acc:" << test_acc << "===" << endl;
            }

        }

        _current_iter += 1;

    }

    void Trainer::train(void)
    {
        // このメソッドを呼び出す場合は必ずTraining Modeにする
        Config::getInstance().set_flag(true);

        for (int i=0; i<_max_iter; i++)
        {
            this->train_step();
        }

        MatrixXd test_X, test_y;
        vector<MatrixXd> val_inputs;

        // 本当はすべてのテストデータを突っ込む形にしたい
        // -> ループで実装し、得られたaccuracyの平均を求めればOK?
        _dataset->next_test(test_X, test_y);
        val_inputs.push_back(test_X);

        double test_acc = _model->accuracy(val_inputs, test_y);

        if (_verbose)
        {
            cout << "========== Final Test Accuracy ==========" << endl;
            cout << "test acc: " << test_acc << endl;
        }

    }

}


#include <QApplication>
#include <QMessageBox>
#include <QMainWindow>
#include "Kernel_type.h"
#include "Scene_surface_mesh_item.h"
#include "Scene_polylines_item.h"

#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/Polygon_mesh_processing/stitch_borders.h>

#include <CGAL/boost/graph/split_graph_into_polylines.h>
#include <CGAL/boost/graph/helpers.h>
#include <boost/graph/filtered_graph.hpp>

using namespace CGAL::Three;
class Polyhedron_demo_polyhedron_stitching_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0" FILE "polyhedron_stitching_plugin.json")

  QAction* actionDetectBorders;
  QAction* actionStitchBorders;
  QAction* actionStitchByCC;
public:
  QList<QAction*> actions() const { return QList<QAction*>() << actionDetectBorders << actionStitchBorders << actionStitchByCC; }
  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* /* m */)
  {
    scene = scene_interface;
    actionDetectBorders= new QAction(tr("Detect Boundaries"), mainWindow);
    actionDetectBorders->setObjectName("actionDetectBorders");
    actionDetectBorders->setProperty("subMenuName", "Polygon Mesh Processing");
    actionStitchBorders= new QAction(tr("Stitch Duplicated Boundaries"), mainWindow);
    actionStitchBorders->setObjectName("actionStitchBorders");
    actionStitchBorders->setProperty("subMenuName", "Polygon Mesh Processing/Repair");

    actionStitchByCC= new QAction(tr("Stitch Borders Per Connected Components"), mainWindow);
    actionStitchByCC->setObjectName("actionStitchByCC");
    actionStitchByCC->setProperty("subMenuName", "Polygon Mesh Processing/Repair");

    autoConnectActions();
  }

  bool applicable(QAction*) const {
    Q_FOREACH(int index, scene->selectionIndices())
    {
      if ( qobject_cast<Scene_surface_mesh_item*>(scene->item(index)) )
        return true;
    }
    return false;
  }

  template <typename Item>
  void on_actionDetectBorders_triggered(Scene_interface::Item_id index);

  template <typename Item>
  void on_actionStitchBorders_triggered(Scene_interface::Item_id index);

  template <typename Item>
  void on_actionStitchByCC_triggered(Scene_interface::Item_id index);

public Q_SLOTS:
  void on_actionDetectBorders_triggered();
  void on_actionStitchBorders_triggered();
  void on_actionStitchByCC_triggered();

}; // end Polyhedron_demo_polyhedron_stitching_plugin

template <typename Item>
void Polyhedron_demo_polyhedron_stitching_plugin::on_actionDetectBorders_triggered(Scene_interface::Item_id index)
{
  typedef typename Item::Face_graph  FaceGraph;
  Item* item = qobject_cast<Item*>(scene->item(index));

  if(item)
    {
      Scene_polylines_item* new_item = new Scene_polylines_item();

      FaceGraph* pMesh = item->polyhedron();
      normalize_border(*pMesh);
      for(auto ed : edges(*pMesh))
      {
        if(pMesh->is_border(ed))
        {
          new_item->polylines.push_back(Scene_polylines_item::Polyline());
          new_item->polylines.back().push_back(pMesh->point(pMesh->source(pMesh->halfedge(ed))));
          new_item->polylines.back().push_back(pMesh->point(pMesh->target(pMesh->halfedge(ed))));
        }
      }

      if (new_item->polylines.empty())
        {
          delete new_item;
        }
      else
        {
          new_item->setName(tr("Boundary of %1").arg(item->name()));
          new_item->setColor(Qt::red);
          scene->addItem(new_item);
          new_item->invalidateOpenGLBuffers();
        }
    }
}

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionDetectBorders_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices()){
    on_actionDetectBorders_triggered<Scene_surface_mesh_item>(index);
  }
}

template <typename Item>
void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchBorders_triggered(Scene_interface::Item_id index)
{
  Item* item =
    qobject_cast<Item*>(scene->item(index));

  if(item){
    typename Item::Face_graph* pMesh = item->polyhedron();
    CGAL::Polygon_mesh_processing::stitch_borders(*pMesh);
    item->invalidateOpenGLBuffers();
    scene->itemChanged(item);
  }
}

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchBorders_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices()){
    on_actionStitchBorders_triggered<Scene_surface_mesh_item>(index);
  }
}

template <typename Item>
void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchByCC_triggered(Scene_interface::Item_id index)
{
  Item* item =
      qobject_cast<Item*>(scene->item(index));

  if(!item)
    return;
  CGAL::Polygon_mesh_processing::stitch_borders(*item->polyhedron(),
                                                CGAL::parameters::apply_per_connected_component(true));
  item->invalidateOpenGLBuffers();
  scene->itemChanged(item);
}

void Polyhedron_demo_polyhedron_stitching_plugin::on_actionStitchByCC_triggered()
{
  Q_FOREACH(int index, scene->selectionIndices()){
    on_actionStitchByCC_triggered<Scene_surface_mesh_item>(index);
  }
}
#include "Polyhedron_stitching_plugin.moc"


#ifndef FORCEFIELD_H
#define FORCEFIELD_H

#include <Eigen/Dense>

namespace SnowSimulator {

class ForceField {
public:
  virtual Eigen::Vector3f force(const Eigen::Vector3f &pos, float mass) const;
  virtual Eigen::Matrix3f gradForce(const Eigen::Vector3f &pos,
                                    float mass) const;
};

} // namespace SnowSimulator

#endif // FORCEFIELD_H


/*! \file demo_2d_lines.cpp
    \brief Demonstration of 2D 'function' lines, for example a least-squares fit line.
    \details Adding lines and curves to a plot.
    \author Paul A. Bristow
    \date Mar 2009
*/

// Copyright Paul A. Bristow 2009

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

#ifdef _MSC_VER
#  pragma warning (disable : 4800) // forcing value to bool 'true' or 'false' (performance warning)
#  pragma warning (disable : 4180) // qualifier applied to function type has no meaning; ignored
#  pragma warning (disable : 4503) // decorated name length exceeded, name was truncated
#  pragma warning (disable : 4512) // assignment operator could not be generated
#  pragma warning (disable : 4100) // unreferenced formal parameter
#endif

//[demo_2d_lines_1

/*`First we need some includes to use Boost.Plot and C++ Standard Library:
*/#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;

#include <boost/svg_plot/show_2d_settings.hpp>
// using boost::svg::show_2d_plot_settings - Only needed for showing which settings in use.

#include <map>
  using std::map;
  using std::multimap;

#include <iostream>
   using std::cout;
   using std::endl;
   using std::boolalpha;
//] [/demo_2d_lines_1]

int main()
{
  try
  {
//[demo_2d_lines_2

/*`
This shows how to add lines to a plot, for example, for example a least-squares 'function' fit line.
The data is roughly a straight line through the origin.
*/

  map<double, double> my_data;
  my_data[-1.1] = -1.2;
  my_data[-2.3] = -2.1;
  my_data[-2.9] = -3.3;
  my_data[-4.1] = -4.3;
  my_data[-5.0] = -5.3;
  my_data[-6.1] = -5.9;
  my_data[0.] = 0.;
  my_data[1.1] = 1.2;
  my_data[2.3] = 2.1;
  my_data[2.9] = 3.3;
  my_data[4.1] = 4.3;
  my_data[5.0] = 5.3;
  my_data[6.1] = 5.9;

  /*`
  First construct, size and draw a simple plot ready to add some sample lines.
  */
  svg_2d_plot my_plot;
  my_plot.size(400, 400);
  my_plot.plot(my_data, "my_data").fill_color(red);

  /*`
  First draw a line using SVG coordinates (rather than the Cartesian coordinates used for user's data - see below).
  Note that for SVG coordinates, Y increases *down* the page, so Y = 0 is the top and Y = 300 is the bottom.
  Default black is provided for color.
  */
  my_plot.draw_line(10, 390, 390, 10);

/*`This line almost reaches the corners of the SVG image, but for plotting XY graphs,
you probably don't want SVG coordinates, but want to use Cartesian coordinates for user's data.
Now draw a blue line using the Cartesian coordinates for user's data,
from the bottom left through the origin of the plot to the top right of the plot.

*/
  my_plot.draw_plot_line(-10, -10, +10, +10, blue);

/*`If you have calculated a confidence interval, you might want to add curved line(s) showing it (still using the Cartesian coordinates).
For example, you can draw a curve (quadratic) through two X, Y pairs using a Bezier curve with the middle point as control point.
*/
  my_plot.draw_plot_curve(-6, -8, 0, +1, +8, +6, red);

/*`Finally write the SVG image file.
[note At present, notes and lines must come after all plot commands to be put in the correct place.]
*/
  my_plot.write("./demo_2d_lines");
  //show_2d_plot_settings(my_plot);
//] [/demo_2d_lines_2]

  }
  catch (const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*
Output :

demo_2d_lines.cpp
Linking...
Embedding manifest...
Autorun "j:\Cpp\SVG\Debug\demo_2d_lines.exe"

*/


#include <iostream>

#include <cstdlib>
#include <cmath>

#include <armadillo>

#include "Conv2D/Mesh.hpp"
#include "Conv2D/EulerDefaultBase.hpp"

double uniform(double a, double b)
{
    return a + ((b - a) * std::rand() / RAND_MAX);
}

double consistencyTestError(double gamma, const EulerDefaultBase &problem)
{
    // construct random unit normal
    const double nx = uniform(-1.0, 1.0);
    const double ny = uniform(-1.0, 1.0);
    const double nl = std::hypot(nx, ny);
    const arma::rowvec n {nx / nl, ny / nl};

    // construct a state
    const double rho = uniform( 0.0, 1.0);
    const double u = uniform(-1.0, 1.0);
    const double v = uniform(-1.0, 1.0);
    const double e = uniform( 0.0, 1.0);
    const double E = e + 0.5 * (u * u + v * v);
    const arma::vec U {rho, rho * u, rho * v, rho * E};

    // compute analytical flux
    const double p = (gamma - 1.0) * rho * e;
    const double H = E + p / rho;
    const arma::vec F {
            n[0] * (rho * u)         + n[1] * (rho * v),
            n[0] * (rho * u * u + p) + n[1] * (rho * u * v),
            n[0] * (rho * u * v)     + n[1] * (rho * v * v + p),
            n[0] * (rho * u * H)     + n[1] * (rho * v * H)};

    // compute Roe flux
    arma::vec FHat(4);
    double s;
    problem.computeRoeFlux(U, U, n, FHat, s);

    // return error
    return arma::norm(F - FHat);
}

double flipTestError(const EulerDefaultBase &problem)
{
    // construct random oppsing unit normals
    const double nx = uniform(-1.0, 1.0);
    const double ny = uniform(-1.0, 1.0);
    const double nl = std::hypot(nx, ny);
    const arma::rowvec nLR { nx / nl,  ny / nl};
    const arma::rowvec nRL {-nx / nl, -ny / nl};

    // construct left state
    const double rhoL = uniform( 0.0, 1.0);
    const double uL = uniform(-1.0, 1.0);
    const double vL = uniform(-1.0, 1.0);
    const double EL = uniform( 0.0, 1.0) + 0.5 * (uL * uL + vL * vL);
    const arma::vec UL {rhoL, rhoL * uL, rhoL * vL, rhoL * EL};

    // construct right state
    const double rhoR = uniform( 0.0, 1.0);
    const double uR   = uniform(-1.0, 1.0);
    const double vR   = uniform(-1.0, 1.0);
    const double ER   = uniform( 0.0, 1.0) + 0.5 * (uR * uR + vR * vR);
    const arma::vec UR {rhoR, rhoR * uR, rhoR * vR, rhoR * ER};

    // temporary variable to store wave speed, not used here!
    double s;

    // compute Roe flux from left to right
    arma::vec FHatLR(4);
    problem.computeRoeFlux(UL, UR, nLR, FHatLR, s);

    // compute Roe flux from right to left
    arma::vec FHatRL(4);
    problem.computeRoeFlux(UR, UL, nRL, FHatRL, s);

    // return error
    return arma::norm(FHatLR + FHatRL);
}

double supersonicTestError(double gamma, const EulerDefaultBase &problem)
{
    // construct normal that points north-east
    const double nx = uniform(0.0, 1.0);
    const double ny = uniform(0.0, 1.0);
    const double nl = std::hypot(nx, ny);
    const arma::rowvec n {nx / nl, ny / nl};

    // construct supersonic state on the left
    const double rhoL = uniform(0.0, 1.0);
    const double uL   = uniform(0.8, 1.0);
    const double vL   = uniform(0.8, 1.0);
    const double eL   = uniform(0.0, 1.0);
    const double EL   = eL + 0.5 * (uL * uL + vL * vL);

    const arma::vec UL {rhoL, rhoL * uL, rhoL * vL, rhoL * EL};

    // construct supersonic state on the right
    const double rhoR = uniform(0.0, 1.0);
    const double uR   = uniform(0.8, 1.0);
    const double vR   = uniform(0.8, 1.0);
    const double eR   = uniform(0.0, 1.0);
    const double ER   = eR + 0.5 * (uR * uR + vR * vR);

    const arma::vec UR {rhoR, rhoR * uR, rhoR * vR, rhoR * ER};

    // compute Roe flux
    arma::vec FHat(4);
    double s;
    problem.computeRoeFlux(UL, UR, n, FHat, s);

    // compute analytical flux on the left
    const double pL = (gamma - 1.0) * rhoL * eL;
    const double HL = EL + pL / rhoL;

    const arma::vec FL {
            n[0] * (rhoL * uL)           + n[1] * (rhoL * vL),
            n[0] * (rhoL * uL * uL + pL) + n[1] * (rhoL * uL * vL),
            n[0] * (rhoL * uL * vL)      + n[1] * (rhoL * vL * vL + pL),
            n[0] * (rhoL * uL * HL)      + n[1] * (rhoL * vL * HL)};

    // return error
    return arma::norm(FHat - FL);
}

int main()
{
    // random seed
    std::srand(1279);

    // this choice is gamma is very important for supersonic test!
    const double gamma = 1.4;

    // set up problem
    EulerDefaultBase problem;
    problem.setSpecificHeatRatio(gamma);

    // report errors
    std::cout << "L2 errors in" << std::endl
              << "  Consistency test: " << std::scientific
              << consistencyTestError(gamma, problem) << std::endl
              << "  Flip test       : " << std::scientific
              << flipTestError(problem) << std::endl
              << "  Supersonic test : " << std::scientific
              << supersonicTestError(gamma, problem) << std::endl;

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// 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 ofthe Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
/// IN THE SOFTWARE.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <utility>
#include <vector>
#include <random>
#include <unordered_map>
#include <boost/progress.hpp>

#include "../index.hpp"

namespace ss {

    using std::vector;

    template<class DataType>
    class KMeansIndex : public Index<DataType> {
    protected:
        /// _center[i] means the i'th center's coordinate
        vector<vector<DataType > >     _centers;
        /// _points[i] means all points' indexes belong to i'th center
        /// _points[i][j] means the j'th point's index belongs to i'th center
        vector<vector<int > >          _points;
    public:
        explicit KMeansIndex(const parameter& para) :
                Index<DataType >(para),
                _centers(para.kmeans_centers),
                _points(para.kmeans_centers) {}

        const vector<vector<DataType > > & get_centers() const { return _centers; }
        const vector<vector<int > >      & get_points()  const { return _points; }

        void reset(int num_centers) {
            this->_centers = vector<vector<DataType > >(num_centers);
            this->_points = vector<vector<int > >(num_centers);
        }

        void set_centers(const vector<vector<DataType > >& centers) {
            _centers = centers;
        }

        void Train(const Matrix<DataType > & data) override {
            Iterate(Visitor<DataType >(data, 0, data.getDim()));
        }

        void Add(const Matrix<DataType > & data) override {
            Assign(Visitor<DataType >(data, 0, data.getDim()));
        }

        void Search(const DataType *query, const std::function<void (int)>& prober) override {
            const vector<int>& idx = _points[NearestCenter(query, _centers[0].size())];
            for (int id : idx) {
                prober(id);
            }
        }

        const vector<int>& Search(const DataType *query)  {
            return  _points[NearestCenter(query, _centers[0].size())];
        }

        /***
         * iteratively update centers and re-assign points
         * @param data
         */
        void Iterate(const Visitor<DataType> & data) {

            /// initialize centers
            /// TODO(Xinyan): should initialized randomly
            for(int i=0; i<_centers.size(); i++) {
                _centers[i] = vector<DataType >(data[i], data[i]+data.getDim());
            }

            boost::progress_display progress(this->_para.iteration);
            for (int iter = 0; iter < this->_para.iteration; ++iter, ++progress) {

                /// Assignment
                Assign(data);
                /// UpdateCenter
                Update(data);
                /// clear points in each centers
                for (int c = 0; c<_points.size(); c++) {
                    _points[c].clear();
                }
            }
        }

        /**
         * re-calculate center by averaging points' coordinate
         */
        void Update(const Visitor<DataType> & data) {
            for (int c = 0; c < _centers.size(); ++c) {

                vector<DataType > sum(data.getDim(), 0.0f);
                for (int p = 0; p < _points[c].size(); ++p) {

                    for (int d = 0; d < data.getDim(); ++d) {
                        sum[d] += data[_points[c][p]][d];        /// add up
                    }
                }

                for (int d = 0; d < data.getDim(); ++d) {
                    _centers[c][d] = sum[d] / _points[c].size(); /// average
                }
            }
        }

        /**
         * assign each point in {@link data} to nearest center
         */
        void Assign(const Visitor<DataType> & data) {
            vector<int > codes(data.getSize());
#pragma omp parallel for
            for (int i = 0; i < data.getSize(); ++i) {
                codes[i] = NearestCenter(data[i], data.getDim());
            }
            for (int i=0; i<data.getSize(); ++i) {
                _points[codes[i]].push_back(i);
            }
        }

        int NearestCenter(const DataType * vector, int dimension) {
            DataType min_distance = ss::EuclidDistance(vector, _centers[0].data(), dimension);
            int nearest_center = 0;
            for (int c = 1; c < _centers.size(); c++) {
                DataType distance = Distance(vector, dimension, c);
                if (distance < min_distance) {
                    min_distance = distance;
                    nearest_center = c;
                }
            }
            return nearest_center;
        }

        /**
         * calculate distances from {@link vector} to each center
         * @return distances within a vector of pair<distance, center>
         */
        std::vector<std::pair<float, int > > ClusterDistance(const DataType *vector, int dimension) {
            std::vector<std::pair<float, int>> dist_centers(this->_centers.size());
            for (int center = 0; center < (this->_centers.size()); ++center) {
                DataType distance = Distance(vector, dimension, center);
                dist_centers[center] = std::make_pair(distance, center);
            }

            return dist_centers;
        }

        inline DataType Distance(const DataType * vector, int dimension, int center) {
            return ss::EuclidDistance(vector, _centers[center].data(), dimension);
        }

    };

} // namespace ss


#ifndef CRADLE_GEOMETRY_GRID_POINTS_HPP
#define CRADLE_GEOMETRY_GRID_POINTS_HPP

#include <boost/iterator/iterator_facade.hpp>
#include <cradle/geometry/regular_grid.hpp>
#include <vector>

namespace cradle {

// A regular_grid_iterator is used for traversing the points on a regular_grid.
template<unsigned N, typename T>
class regular_grid_iterator : public boost::iterator_facade<
                                  regular_grid_iterator<N, T>,
                                  vector<N, T> const,
                                  boost::random_access_traversal_tag>
{
 public:
    regular_grid_iterator()
    {
    }
    regular_grid_iterator(
        regular_grid<N, T> const* grid,
        vector<N, unsigned> const& i,
        vector<N, T> const& p)
        : grid(grid), i(i), p(p)
    {
    }

 private:
    friend class boost::iterator_core_access;

    vector<N, T> const&
    dereference() const
    {
        return p;
    }

    bool
    equal(regular_grid_iterator const& other) const
    {
        return this->i == other.i;
    }

    void
    increment();
    void
    decrement();

    void
    advance(typename regular_grid_iterator::difference_type n);

    typename regular_grid_iterator::difference_type
    distance_to(regular_grid_iterator const& other) const;

    regular_grid<N, T> const* grid;
    vector<N, unsigned> i;
    vector<N, T> p;
};

template<unsigned N, typename T>
class regular_grid_point_list
{
 public:
    regular_grid_point_list()
    {
    }
    regular_grid_point_list(regular_grid<N, T> const& grid) : grid_(grid)
    {
    }

    typedef regular_grid_iterator<N, T> iterator;
    typedef iterator const_iterator;

    const_iterator
    begin() const
    {
        return const_iterator(
            &grid_, uniform_vector<N, unsigned>(0), grid_.p0);
    }
    const_iterator
    end() const
    {
        vector<N, unsigned> i = uniform_vector<N, unsigned>(0);
        i[N - 1] = grid_.n_points[N - 1];
        return const_iterator(
            &grid_, i, grid_.p0 + cradle::vector<N, T>(i) * grid_.spacing);
    }

    typedef size_t size_type;
    size_type
    size() const
    {
        return product(grid_.n_points);
    }

 private:
    regular_grid<N, T> grid_;
};

template<unsigned N, typename T>
regular_grid_point_list<N, T>
make_grid_point_list(regular_grid<N, T> const& grid)
{
    return regular_grid_point_list<N, T>(grid);
}

// Get the list of points on a grid.
api(fun with(N : 1, 2, 3, 4; T : double))
template<unsigned N, typename T>
// List of points on the N dimensional grid, with N being 1 thru 4
std::vector<cradle::vector<N, T>>
get_points_on_grid(
    // The grid over which to compute the point positions
    regular_grid<N, T> const& grid)
{
    auto lazy = make_grid_point_list(grid);
    std::vector<cradle::vector<N, T>> eager;
    eager.reserve(lazy.size());
    for (auto i = lazy.begin(); i != lazy.end(); ++i)
        eager.push_back(*i);
    return eager;
}

namespace impl {

template<unsigned N, typename T, unsigned I_>
struct regular_grid_iterator_incrementer
{
    static void
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned>& i,
        vector<N, T>& p)
    {
        static unsigned const I = N - I_;
        ++i[I];
        if (i[I] == grid.n_points[I])
        {
            i[I] = 0;
            p[I] = grid.p0[I];
            regular_grid_iterator_incrementer<N, T, N - (I + 1)>::apply(
                grid, i, p);
        }
        else
            p[I] += grid.spacing[I];
    }
};
template<unsigned N, typename T>
struct regular_grid_iterator_incrementer<N, T, 1>
{
    static void
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned>& i,
        vector<N, T>& p)
    {
        static unsigned const I = N - 1;
        ++i[I];
        p[I] += grid.spacing[I];
    }
};
} // namespace impl

template<unsigned N, typename T>
void
regular_grid_iterator<N, T>::increment()
{
    impl::regular_grid_iterator_incrementer<N, T, N>::apply(*grid, i, p);
}

namespace impl {

template<unsigned N, typename T, unsigned I_>
struct regular_grid_iterator_decrementer
{
    static void
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned>& i,
        vector<N, T>& p)
    {
        static unsigned const I = N - I_;
        if (i[I] == 0)
        {
            i[I] = grid.n_points[I] - 1;
            p[I] = grid.p0[I] + i[I] * grid.spacing[I];
            regular_grid_iterator_decrementer<N, T, N - (I + 1)>::apply(
                grid, i, p);
        }
        else
        {
            --i[I];
            p[I] -= grid.spacing[I];
        }
    }
};
template<unsigned N, typename T>
struct regular_grid_iterator_decrementer<N, T, 1>
{
    static void
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned>& i,
        vector<N, T>& p)
    {
        static unsigned const I = N - 1;
        --i[I];
        p[I] -= grid.spacing[I];
    }
};
} // namespace impl

template<unsigned N, typename T>
void
regular_grid_iterator<N, T>::decrement()
{
    impl::regular_grid_iterator_decrementer<N, T, N>::apply(*grid, i, p);
}

namespace impl {

template<unsigned N, typename T, unsigned I_>
struct regular_grid_iterator_advancer
{
    template<typename Difference>
    static void
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned>& i,
        vector<N, T>& p,
        Difference n)
    {
        static unsigned const I = N - I_;
        Difference t = static_cast<Difference>(i[I]) + n;
        Difference r = t % static_cast<Difference>(grid.n_points[I]);
        Difference q = t / static_cast<Difference>(grid.n_points[I]);
        if (r < 0)
        {
            r = grid.n_points[I] - r;
            --q;
        }
        i[I] = unsigned(r);
        p[I] = grid.p0[I] + i[I] * grid.spacing[I];
        if (q != 0)
        {
            regular_grid_iterator_advancer<N, T, N - (I + 1)>::apply(
                grid, i, p, q);
        }
    }
};
template<unsigned N, typename T>
struct regular_grid_iterator_advancer<N, T, 1>
{
    template<typename Difference>
    static void
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned>& i,
        vector<N, T>& p,
        Difference n)
    {
        static unsigned const I = N - 1;
        i[I] += unsigned(n);
        p[I] = grid.p0[I] + i[I] * grid.spacing[I];
    }
};
} // namespace impl

template<unsigned N, typename T>
void
regular_grid_iterator<N, T>::advance(
    typename regular_grid_iterator<N, T>::regular_grid_iterator::
        difference_type n)
{
    impl::regular_grid_iterator_advancer<N, T, N>::apply(*grid, i, p, n);
}

namespace impl {

template<typename Difference, unsigned N, typename T, unsigned I_>
struct regular_grid_iterator_distance
{
    static Difference
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned> const& a,
        vector<N, unsigned> const& b)
    {
        static unsigned const I = N - I_;
        return (static_cast<Difference>(b[I]) - static_cast<Difference>(a[I]))
               + regular_grid_iterator_distance<
                     Difference,
                     N,
                     T,
                     N - (I + 1)>::apply(grid, a, b)
                     * grid.n_points[I];
    }
};
template<typename Difference, unsigned N, typename T>
struct regular_grid_iterator_distance<Difference, N, T, 1>
{
    static Difference
    apply(
        regular_grid<N, T> const& grid,
        vector<N, unsigned> const& a,
        vector<N, unsigned> const& b)
    {
        static unsigned const I = N - 1;
        return (static_cast<Difference>(b[I]) - static_cast<Difference>(a[I]));
    }
};
} // namespace impl

template<unsigned N, typename T>
auto
regular_grid_iterator<N, T>::distance_to(regular_grid_iterator const& other)
    const -> typename regular_grid_iterator::difference_type
{
    assert(*grid == *other.grid);
    return impl::regular_grid_iterator_distance<
        typename regular_grid_iterator<N, T>::difference_type,
        N,
        T,
        N>::apply(*grid, i, other.i);
}

} // namespace cradle

#endif


//
// boost/radix/static_ibitstream_lsb.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_RADIX_STATICIBITSTREAMLSB_HPP
#define BOOST_RADIX_STATICIBITSTREAMLSB_HPP

#include <boost/radix/common.hpp>

#include <boost/radix/bitmask.hpp>
#include <boost/radix/detail/bits.hpp>

#include <boost/cstdint.hpp>

namespace boost { namespace radix {

template <std::size_t Bits>
class static_ibitstream_lsb {
 public:
  template <typename RandomAccessIterator, typename UnpackedSegment>
  void operator()(RandomAccessIterator bytes, UnpackedSegment& u) const {
    using namespace bits;
    std::size_t const unpacked_size = to_unpacked_segment_size<Bits>::value;
    std::size_t const packed_size   = to_packed_segment_size<Bits>::value;

    BOOST_STATIC_ASSERT(packed_size <= sizeof(boost::uint64_t));

    boost::uint64_t packed = 0;
    for(std::size_t i = 0; i < packed_size; ++i) {
      packed |= boost::uint64_t(*bytes++) << (i * 8);
    }

    for(std::size_t i = 0; i < unpacked_size; ++i) {
      u[i] = static_cast<char_type>(packed & mask<Bits>::value);
      packed >>= Bits;
    }
  }
};

}} // namespace boost::radix

#endif // BOOST_RADIX_STATICIBITSTREAMLSB_HPP


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/equal.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/zip_with.hpp>

#include <functional>
namespace hana = boost::hana;

//
// Example of implementing basic dimensional analysis using Hana
//

// base dimensions                              M  L  T  I  K  J  N
using mass        = decltype(hana::tuple_c<int, 1, 0, 0, 0, 0, 0, 0>);
using length      = decltype(hana::tuple_c<int, 0, 1, 0, 0, 0, 0, 0>);
using time_       = decltype(hana::tuple_c<int, 0, 0, 1, 0, 0, 0, 0>);
using charge      = decltype(hana::tuple_c<int, 0, 0, 0, 1, 0, 0, 0>);
using temperature = decltype(hana::tuple_c<int, 0, 0, 0, 0, 1, 0, 0>);
using intensity   = decltype(hana::tuple_c<int, 0, 0, 0, 0, 0, 1, 0>);
using amount      = decltype(hana::tuple_c<int, 0, 0, 0, 0, 0, 0, 1>);

// composite dimensions
using velocity     = decltype(hana::tuple_c<int, 0, 1, -1, 0, 0, 0, 0>); // M/T
using acceleration = decltype(hana::tuple_c<int, 0, 1, -2, 0, 0, 0, 0>); // M/T^2
using force        = decltype(hana::tuple_c<int, 1, 1, -2, 0, 0, 0, 0>); // ML/T^2

template <typename Dimensions>
struct quantity {
    double value_;

    explicit quantity(double v) : value_(v) { }

    template <typename OtherDimensions>
    explicit quantity(quantity<OtherDimensions> other)
      : value_(other.value_)
    {
      static_assert(Dimensions{} == OtherDimensions{},
        "Constructing quantities with incompatible dimensions!");
    }

    explicit operator double() const { return value_; }
};

template <typename D1, typename D2>
auto operator*(quantity<D1> a, quantity<D2> b) {
    using D = decltype(hana::zip_with(std::plus<>{}, D1{}, D2{}));
    return quantity<D>{static_cast<double>(a) * static_cast<double>(b)};
}

template <typename D1, typename D2>
auto operator/(quantity<D1> a, quantity<D2> b) {
    using D = decltype(hana::zip_with(std::minus<>{}, D1{}, D2{}));
    return quantity<D>{static_cast<double>(a) / static_cast<double>(b)};
}

int main() {
    quantity<mass>         m{10.3};
    quantity<length>       d{3.6};
    quantity<time_>        t{2.4};
    quantity<velocity>     v{d / t};
    quantity<acceleration> a{3.9};
    quantity<force>        f{m * a};
}


#include <math.h>
#include <vfh_rover/PolarHistogram.h>
#include <vfh_rover/Vehicle.h>
#include <string>
#include <assert.h>
#include <iostream>
#include <cmath>
#include <pcl/point_types.h>
#include <geometry_msgs/Pose.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;

PolarHistogram::PolarHistogram(float alpha,
    float ox, float oy, float oz):
  alpha(alpha), ox(ox), oy(oy), oz(oz)
{
  data = new float[getWidth() * getHeight()]();
  for (int i = 0; i < getWidth() * getHeight(); i++) {
    data[i] = NAN;
  }
}
//Modified... Revert to the commented if problem occurs
int PolarHistogram::getI(float x, float y) {
  //return modulus(floor(atan2(x-ox, y-oy)/alpha), getWidth());
  return floor(atan2(x-ox, y-oy)/alpha);
}

int PolarHistogram::getJ(float x, float y, float z) {
  float p = sqrt(pow((x-ox), 2) + pow((y-oy), 2));
  return floor((atan2(z-oz, p)+M_PI/2)/alpha);
}

// j = 0 = top
float PolarHistogram::getValue(int i, int j) {
  int az = modulus(i, getWidth());
  int el = clip(j, 0, getHeight()-1);
  if (j - el > 0) {
    // if el overflows the top and goes over to the other side
    // j is negative
    // el is 0
    el = getHeight() - (j-el);
    az = modulus(az, getWidth());
  } else if (j - el > 0) {
    // if el overflows the bot and goes over to the other side
    el = -(j-el);
    az = modulus(az+getWidth()/2, getWidth());
  }
  //std::cout << "g: " << (el*getWidth())+az << std::endl;
  return this->data[(el*getWidth())+az];
}

void PolarHistogram::setValue(int i, int j, float val) {
  int az = modulus(i, getWidth());
  int el = clip(j, 0, getHeight()-1);
  if (j - el > 0) {
    // if el overflows the top and goes over to the other side
    // j is negative
    // el is 0
    el = getHeight() - (j-el);
    az = modulus(az, getWidth());
  } else if (j - el > 0) {
    // if el overflows the bot and goes over to the other side
    el = -(j-el);
    az = modulus(az+getWidth()/2, getWidth());
  }
  //std::cout << "HI3 " << i << ", " << j << ", " << getHeight() << std::endl;
  //std::cout << "s: " << (el*getWidth())+az << std::endl;
  this->data[(el*getWidth())+az] = val;
}

void PolarHistogram::setValues(float* vals, int x, int y, int width, int height) {
  float el, az;
  for (int i=x; i<x+width; i++) {
    for (int j=y; j<y+height; j++) {
      setValue(i, j, vals[(j-y)*width+(i-x)]);
    }
  }
}

void PolarHistogram::addValues(float* vals, int x, int y, int width, int height) {
  float el, az;
  for (int i=x; i<x+width; i++) {
    for (int j=y; j<y+height; j++) {
      //setValue(i, j, getValue(i, j) + vals[(j-y)*width+(i-x)]);
      addValue(i, j, vals[(j-y)*width+(i-x)]);
    }
  }
}

void PolarHistogram::addValues(float val, int x, int y, int width, int height) {
  float el, az;
  for (int i=x; i<x+width; i++) {
    for (int j=y; j<y+height; j++) {
      //setValue(i, j, getValue(i, j) + val);
      addValue(i, j, val);
    }
  }
}

void PolarHistogram::getValues(float* return_vals, int x, int y, int width, int height) {
  float el, az;
  for (int i=x; i<x+width; i++) {
    for (int j=y; j<y+height; j++) {
      return_vals[(j-y)*width+(i-x)] = getValue(i, j);
    }
  }
}

void PolarHistogram::addValue(float i, float j, float val) {
  float v = getValue(i, j);
  // Uninitallized values are -1. If uninitialized, intialize on add
  if (v != v)
    v = 0;
  setValue(i, j, v + val);
}

int PolarHistogram::getN() {
  int n = 0;
  for (int i = 0; i < getWidth()*getHeight(); i++) {
    if (data[i] == data[i]) {
      n++;
    }
  }
  return n;
}

float PolarHistogram::mean() {
  float sum = 0;
  int n = 0;
  for (int i = 0; i < getWidth()*getHeight(); i++) {
    if (data[i] > 0 && data[i] == data[i]) {
      sum += data[i];
      n++;
    }
  }
  if (n == 0)
    return 1;
  return sum / n;
}

float PolarHistogram::std() {
  float m = mean();
  float sum = 0;
  int n = 0;
  for (int i = 0; i < getWidth()*getHeight(); i++) {
    if (data[i] > 0) {
      sum += pow(m - data[i], 2);
      n++;
    }
  }
  if (n == 0)
    return 99;
  return sum / n;
}

float PolarHistogram::getMeanArea() {
  float radius = 1;

  float area = 4*M_PI*pow(radius, 2);
  return area/getN();
}

float PolarHistogram::getArea(int elevation) {
  float r1, r2, h1, h2, r, h;
  float elevationF = ((getHeight()/2 - elevation) >= 0 ? elevation : abs(getHeight() - elevation));
  h1 = cos(alpha * elevationF);
  h2 = cos(alpha * (elevationF+1));
  r1 = sin(alpha * elevationF);
  r2 = sin(alpha * (elevationF+1));

  r = (r1+r2)/2;
  h = (h1-h2);
  std::cout << "Radius is ------------: " << r << " Height is ---------: "<< h << "At elevation: " << elevationF<<std::endl;
  return 2*M_PI*(r*h)/getWidth();
}

int PolarHistogram::getWidth() {
  return int(ceil((2*M_PI)/alpha));
}

int PolarHistogram::getHeight() {
  return int(ceil((M_PI)/alpha));
}

std::string PolarHistogram::displayString() {
  std::string s;
  for (int j=0; j<this->getHeight(); j++) {
    for (int i=0; i<this->getWidth(); i++) {
      s.append(std::to_string(this->getValue(i, j)));
      s.append(", ");
    }
    s.append("\n");
  }
  return s;
}

RGBPointCloud::Ptr PolarHistogram::displayCloud(float radius) {
  RGBPointCloud::Ptr pc (new RGBPointCloud);
  float m = mean();
  for (int i=0; i<this->getWidth(); i++) {
    for (int j=0; j<this->getHeight(); j++) {
      // Determine angle on circle
      float val = getValue(i, j) * (255/m);
      float az = alpha*modulus(i-getWidth()/2, getWidth()) + alpha/2;
      float el = alpha*modulus(j-getHeight()/2, getHeight()) + alpha/2;

      // Determine point coords
      pcl::PointXYZRGB p;
      float sign = (el < M_PI/2) ? 1 : -1;
      p.x = -sign*radius*cos(el)*sin(az)+ox;
      p.y = -sign*radius*cos(el)*cos(az)+oy;
      p.z = sign*radius*sin(el)+oz;

      // Clip colors
      if (val != val)
        val = 255;
      float color = val < 0 ? 0 : val;
      color = color >= 255 ? 255 : color;

      p.r = p.g = p.b = color;
      pc->points.push_back(p);
    }
  }
  return pc;
}


#ifndef quantlib_floatfloatds_swap_hpp
#define quantlib_floatfloatds_swap_hpp

#include <ql/instruments/swap.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/schedule.hpp>
#include <boost/optional.hpp>

namespace QuantLib {

    class InterestRateIndex;

	//It broadens the original FloatFloatSwap. Not possible to inherit from FloatFloatSwap because the latter's contructor involves init() which sets the types of coupons
    class FloatFloatSwapDS : public Swap {
      public:
        class arguments;
        class results;
        class engine;
        FloatFloatSwapDS(
            const VanillaSwap::Type type,
            const Real nominal1,
            const Real nominal2,
            const Schedule& schedule1,
            const ext::shared_ptr<InterestRateIndex>& index1,
            const DayCounter& pmtDayCount1,
            Natural pmtLag1,
            bool inArrears1,
            const Schedule& schedule2,
            const ext::shared_ptr<InterestRateIndex>& index2,
            const DayCounter& pmtDayCount2,
            Natural pmtLag2,
            bool inArrears2,
            bool intermediateCapitalExchange = false,
            bool finalCapitalExchange = false,
            Real gearing1 = 1.0,
            Real spread1 = 0.0,
            Real cappedRate1 = Null<Real>(),
            Real flooredRate1 = Null<Real>(),
            Real gearing2 = 1.0,
            Real spread2 = 0.0,
            Real cappedRate2 = Null<Real>(),
            Real flooredRate2 = Null<Real>(),
            const boost::optional<BusinessDayConvention>& pmtConv1 = boost::none,
            const boost::optional<BusinessDayConvention>& pmtConv2 = boost::none,
			Calendar const & pmtCal1 = NullCalendar(),
			Calendar const & pmtCal2 = NullCalendar());

        FloatFloatSwapDS(
            VanillaSwap::Type type,
            const std::vector<Real>& nominal1,
            const std::vector<Real>& nominal2,
            const Real nominalFinal1,
            const Real nominalFinal2,
            const Schedule& schedule1,
            const ext::shared_ptr<InterestRateIndex>& index1,
            const DayCounter& pmtDayCount1,
            Natural pmtLag1,
            bool inArrears1,
            const Schedule& schedule2,
            const ext::shared_ptr<InterestRateIndex>& index2,
            const DayCounter& pmtDayCount2,
            Natural pmtLag2,
            bool inArrears2,
            bool intermediateCapitalExchange = false,
            bool finalCapitalExchange = false,
            const std::vector<Real>& gearing1 = std::vector<Real>(),
            const std::vector<Real>& spread1 = std::vector<Real>(),
            const std::vector<Real>& cappedRate1 = std::vector<Real>(),
            const std::vector<Real>& flooredRate1 = std::vector<Real>(),
            const std::vector<Real>& gearing2 = std::vector<Real>(),
            const std::vector<Real>& spread2 = std::vector<Real>(),
            const std::vector<Real>& cappedRate2 = std::vector<Real>(),
            const std::vector<Real>& flooredRate2 = std::vector<Real>(),
            const boost::optional<BusinessDayConvention>& pmtConv1 = boost::none,
            const boost::optional<BusinessDayConvention>& pmtConv2 = boost::none,
			Calendar const & pmtCal1 = NullCalendar(),
			Calendar const & pmtCal2 = NullCalendar());

        //! \name Inspectors
        //@{
        VanillaSwap::Type type() const;
        const std::vector<Real> &nominal1() const;
        const std::vector<Real> &nominal2() const;

		Real const nominalFinal1() const;
		Real const nominalFinal2() const;

        const Schedule &schedule1() const;
        const Schedule &schedule2() const;

        const ext::shared_ptr<InterestRateIndex> &index1() const;
        const ext::shared_ptr<InterestRateIndex> &index2() const;

        std::vector<Real> spread1() const;
        std::vector<Real> spread2() const;

        std::vector<Real> gearing1() const;
        std::vector<Real> gearing2() const;

        std::vector<Rate> cappedRate1() const;
        std::vector<Rate> flooredRate1() const;
        std::vector<Rate> cappedRate2() const;
        std::vector<Rate> flooredRate2() const;

        const DayCounter &pmtDayCount1() const;
        const DayCounter &pmtDayCount2() const;

		Natural const pmtLag1() const;
		Natural const pmtLag2() const;

		bool const inArrears1() const;
		bool const inArrears2() const;

        BusinessDayConvention pmtConv1() const;
        BusinessDayConvention pmtConv2() const;

        Calendar const & pmtCal1() const;
        Calendar const & pmtCal2() const;

        const Leg &leg1() const;
        const Leg &leg2() const;
        //@}

        //! \name Results
        //@{
        //@}
        // other
        void setupArguments(PricingEngine::arguments *args) const;
        void fetchResults(const PricingEngine::results *) const;

      private:
        void init(boost::optional<BusinessDayConvention> pmtConv1,
                  boost::optional<BusinessDayConvention> pmtConv2);
        void setupExpired() const;
        VanillaSwap::Type type_;
        std::vector<Real> nominal1_, nominal2_;
		Real nominalFinal1_, nominalFinal2_;//Deriscope: Added so that the amort cash flows associated with the last period are also processed
        Schedule schedule1_, schedule2_;
        ext::shared_ptr<InterestRateIndex> index1_, index2_;
        std::vector<Real> gearing1_, gearing2_, spread1_, spread2_;
        std::vector<Real> cappedRate1_, flooredRate1_, cappedRate2_,
            flooredRate2_;
        DayCounter pmtDayCount1_, pmtDayCount2_;
		Natural pmtLag1_, pmtLag2_;
        bool inArrears1_, inArrears2_;
        std::vector<bool> isRedemptionFlow1_, isRedemptionFlow2_;
        BusinessDayConvention pmtConv1_, pmtConv2_;
		Calendar pmtCal1_, pmtCal2_;
        const bool intermediateCapitalExchange_, finalCapitalExchange_;
    };

    //! %Arguments for float float swap calculation
    class FloatFloatSwapDS::arguments : public Swap::arguments {
      public:
        arguments() : type(VanillaSwap::Receiver) {}
        VanillaSwap::Type type;
        std::vector<Real> nominal1, nominal2;
		Real nominalFinal1, nominalFinal2;

        std::vector<Date> leg1ResetDates, leg1FixingDates, leg1PayDates;
        std::vector<Date> leg2ResetDates, leg2FixingDates, leg2PayDates;

        std::vector<Real> leg1Spreads, leg2Spreads, leg1Gearings, leg2Gearings;
        std::vector<Real> leg1CappedRates, leg1FlooredRates, leg2CappedRates,
            leg2FlooredRates;

        std::vector<Real> leg1Coupons, leg2Coupons;
        std::vector<Real> leg1AccrualTimes, leg2AccrualTimes;

        ext::shared_ptr<InterestRateIndex> index1, index2;

        std::vector<bool> leg1IsRedemptionFlow, leg2IsRedemptionFlow;

        void validate() const;
    };

    //! %Results from float float swap calculation
    class FloatFloatSwapDS::results : public Swap::results {
      public:
        void reset();
    };

    class FloatFloatSwapDS::engine
        : public GenericEngine<FloatFloatSwapDS::arguments,
                               FloatFloatSwapDS::results> {};

    // inline definitions

    inline VanillaSwap::Type FloatFloatSwapDS::type() const { return type_; }

    inline const std::vector<Real> &FloatFloatSwapDS::nominal1() const {
        return nominal1_;
    }

    inline const std::vector<Real> &FloatFloatSwapDS::nominal2() const {
        return nominal2_;
    }

    inline Real const FloatFloatSwapDS::nominalFinal1() const { return nominalFinal1_; }
    inline Real const FloatFloatSwapDS::nominalFinal2() const { return nominalFinal2_; }

    inline const Schedule &FloatFloatSwapDS::schedule1() const {
        return schedule1_;
    }

    inline const Schedule &FloatFloatSwapDS::schedule2() const {
        return schedule2_;
    }

    inline const ext::shared_ptr<InterestRateIndex> &
    FloatFloatSwapDS::index1() const {
        return index1_;
    }

    inline const ext::shared_ptr<InterestRateIndex> &
    FloatFloatSwapDS::index2() const {
        return index2_;
    }

    inline std::vector<Real> FloatFloatSwapDS::spread1() const { return spread1_; }

    inline std::vector<Real> FloatFloatSwapDS::spread2() const { return spread2_; }

    inline std::vector<Real> FloatFloatSwapDS::gearing1() const { return gearing1_; }

    inline std::vector<Real> FloatFloatSwapDS::gearing2() const { return gearing2_; }

    inline std::vector<Real> FloatFloatSwapDS::cappedRate1() const { return cappedRate1_; }

    inline std::vector<Real> FloatFloatSwapDS::cappedRate2() const { return cappedRate2_; }

    inline std::vector<Real> FloatFloatSwapDS::flooredRate1() const { return flooredRate1_; }

    inline std::vector<Real> FloatFloatSwapDS::flooredRate2() const { return flooredRate2_; }

    inline const DayCounter &FloatFloatSwapDS::pmtDayCount1() const {
        return pmtDayCount1_;
    }

    inline const DayCounter &FloatFloatSwapDS::pmtDayCount2() const {
        return pmtDayCount2_;
    }

	inline Natural const FloatFloatSwapDS::pmtLag1() const {
        return pmtLag1_;
	}

	inline Natural const FloatFloatSwapDS::pmtLag2() const {
        return pmtLag2_;
	}

	inline bool const FloatFloatSwapDS::inArrears1() const {
        return inArrears1_;
	}

	inline bool const FloatFloatSwapDS::inArrears2() const {
        return inArrears2_;
	}

    inline BusinessDayConvention FloatFloatSwapDS::pmtConv1() const {
        return pmtConv1_;
    }

    inline BusinessDayConvention FloatFloatSwapDS::pmtConv2() const {
        return pmtConv2_;
    }

    inline Calendar const & FloatFloatSwapDS::pmtCal1() const {
        return pmtCal1_;
    }

    inline Calendar const & FloatFloatSwapDS::pmtCal2() const {
        return pmtCal2_;
    }

    inline const Leg &FloatFloatSwapDS::leg1() const { return legs_[0]; }

    inline const Leg &FloatFloatSwapDS::leg2() const { return legs_[1]; }
}

#endif


/*
 * SphereInitializer.hpp
 *
 *  Created on: Nov 8, 2012
 *      Author: petr
 */
#pragma once
#ifndef SPHEREINITIALIZER_HPP_
#define SPHEREINITIALIZER_HPP_

#include <petscdmadda.h>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "Interface.hpp"

class SphereInitializer{

    // where the sphere is located
    Eigen::Vector3d center;

    // how big is the sphere
    PetscReal              radius;

    size_t                 d;

    // geometry is stores here
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > geometry;

    // select which boundaries need to be initialized
    int selectBoundaries(const Box<double, 3>& procAABB);

    // defines the distance to the sphere
    PetscReal sphereEquation(const Eigen::Vector3d& point);

    // initialize the geometry (in this case creates the sphere)
    void initializeGeometry();

public:

    SphereInitializer(const Eigen::Vector3d& center, PetscReal radius);

    // This is where the magic happens and the initializator puts the data in
    template <typename type, int dim>
    void operator() (Interface<type, dim>& interface, bool initAll);

};

///===========================================
///              Implementation
///===========================================

SphereInitializer::SphereInitializer(const Eigen::Vector3d& center, PetscReal radius) {

    this->center = center;
    this->radius = radius;
    this->d = center.size();
    initializeGeometry();

}

template <typename type, int dim>
void SphereInitializer::operator ()(Interface<type, dim>& interface, bool initAll) {

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > boundaryCells;    // xyz coordinated to compute the distance to

    int                        myBoundaries;     // local boundaries to init

    MPI_Group worldGroup;
    MPI_Comm_group(MPI_COMM_WORLD, &worldGroup);
    // perpendicular distance computed towards the triangle, it is used when we are no longer sure, which normal shall we use

    // initialize the data

    Vec           localData = interface.getLocalData();
    double***     data_ptr;
    int           x, y, z, m, n, p;
    int           vecSize;
    DM            cda;
    Vec           gc;
    DMDACoor3d*** coors;

    VecSet(localData, std::numeric_limits<double>::max());
//    VecSet(localData, -0.5);
    VecGetLocalSize(localData, &vecSize);
    assert( vecSize > 0 );

    int myWorldRank;
    int nProcsInWorld;

    MPI_Comm_rank(MPI_COMM_WORLD, &myWorldRank);
    MPI_Comm_size(MPI_COMM_WORLD, &nProcsInWorld);

    const Grid<type, dim>& gr = interface.getGrid();

    DMDAVecGetArray(gr.getDA(), localData, &data_ptr);

    DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);

    DMGetCoordinatesLocal(gr.getDA() ,&gc);
    DMGetCoordinateDM(gr.getDA(), &cda);
    DMDAVecGetArray(cda, gc, &coors);

    if (initAll) {
        // initialize the whole domain

        for (int i = x; i < x+m; ++i) {
            for (int j = y; j < y+n; ++j) {
                for (int k = z; k < z+p; ++k) {
                    data_ptr[k][j][i] =
                        sphereEquation(
                            Eigen::Vector3d(coors[k][j][i].x,
                                            coors[k][j][i].y,
                                            coors[k][j][i].z)
                        );

                }
            }
        }

    } else {
        // insert sphere boundaries
    	double    narrowBand = gr.getDx(0) * 3;
        for (int i = x; i < x+m; ++i) {
            for (int j = y; j < y+n; ++j) {
                for (int k = z; k < z+p; ++k) {
                    double dist =
                        sphereEquation(
                            Eigen::Vector3d(coors[k][j][i].x,
                                            coors[k][j][i].y,
                                            coors[k][j][i].z)
                        );
                    if (fabs(dist) > narrowBand) {
                        continue;
                    }
                    data_ptr[k][j][i] = dist;
                }
            }
        }

        // boundaries
        int gh = 1;
        int boundaries = 63;//selectBoundaries( gr.getNodeSpan(myWorldRank) );

        if ( boundaries & 1 ) {
            // this means lets initialize left side
            for (int i = x; i < x+gh; ++i) {
                for (int j = y; j < y+n; ++j) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j][i+1] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j][i+1].x,
                                                                coors[k][j][i+1].y,
                                                                coors[k][j][i+1].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 2 ) {
            // this means lets initialize right side
            for (int i = x+m-1; i > x+m-1-gh; --i) {
                for (int j = y; j < y+n; ++j) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j][i-1] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j][i-1].x,
                                                                coors[k][j][i-1].y,
                                                                coors[k][j][i-1].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 4 ) {
            // this means initialize the bottom side
            for (int j = y; j < y+gh; ++j) {
                for (int i = x; i < x+m; ++i) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j+1][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j+1][i].x,
                                                                coors[k][j+1][i].y,
                                                                coors[k][j+1][i].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 8 ) {
            // this means initialize the top side
            for (int j = y+n-1; j > y+n-1-gh; --j) {
                for (int i = x; i < x+m; ++i) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j-1][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j-1][i].x,
                                                                coors[k][j-1][i].y,
                                                                coors[k][j-1][i].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 16 ) {
            // this means initialize the front side
            for (int k = z; k < z+gh; ++k) {
                for (int i = x; i < x+m; ++i) {
                    for (int j = y; j < y+n; ++j) {
                        data_ptr[k+1][j][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k+1][j][i].x,
                                                                coors[k+1][j][i].y,
                                                                coors[k+1][j][i].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 32 ) {
            // this means initialize the far side
            for (int k = z+p-1; k > z+p-1-gh; --k) {
                for (int i = x; i < x+m; ++i) {
                    for (int j = y; j < y+n; ++j) {
                        data_ptr[k-1][j][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k-1][j][i].x,
                                                                coors[k-1][j][i].y,
                                                                coors[k-1][j][i].z)
                                                );
                    }
                }
            }
        }

    }

    DMDAVecRestoreArray(cda, gc, &coors);
    DMDAVecRestoreArray(gr.getDA(), localData, &data_ptr);

}

PetscReal SphereInitializer::sphereEquation(const Eigen::Vector3d& point) {
    // this function really speaks for itself. It is just sphere equation

    PetscReal dist = 0;

    dist = sqrt( (point[0] - center[0])*(point[0] - center[0]) +
                 (point[1] - center[1])*(point[1] - center[1]) +
                 (point[2] - center[2])*(point[2] - center[2])) - radius;

    return dist;

}

int SphereInitializer::selectBoundaries(const Box<double, 3>& procAABB) {
    // chooses which boundaries of the domain have to be initialized
    // ordering : [left, right, bottom, top, front, back]

    int selectedBoundaries_int = 0;

    for (int i = 0; i < 3; ++i) {
        //gr->getLocalGhostedMax(i) <= dataProvider->getMaxX(i)
        if ( procAABB.maxX(i) <= (center[i]+radius) ) {
            selectedBoundaries_int |= ( 1 << (i*2 + 1) );
        }
        // gr->getLocalGhostedMin(i) >= dataProvider->getMinX(i)
        if ( procAABB.minX(i) >= (center[i]-radius) ) {
            selectedBoundaries_int |= ( 1 << (i*2) );
        }
    }

    return selectedBoundaries_int;

}

void SphereInitializer::initializeGeometry() {
    // creates the sphere points in Nd

    int    numberOfPoints = 300;

    // x(t) = center(1) - radius*cos(angle_step_1 * t)*sin(angle_step_2 * t);
    // y(t) = center(2) - radius*sin(angle_step_1 * t)*cos(angle_step_2 * t);
    // z(t) =             radius*                      cos(angle_step_2 * t);

    double angle_step_1 = (2*3.1415926535897) / numberOfPoints;
    double angle_step_2 = (  3.1415926535897) / numberOfPoints;

    geometry.reserve(numberOfPoints*numberOfPoints);

    for (int i = 0; i < numberOfPoints; ++i) {

        for (int j = 0; j < numberOfPoints; ++j) {

            double x = center[0] + radius*cos(angle_step_1 * i)*sin(angle_step_2*j);
            double y = center[1] + radius*sin(angle_step_1 * i)*sin(angle_step_2*j);
            double z = center[2] + radius*cos(angle_step_2*j);

            geometry.push_back( Eigen::Vector3d(x, y, z) );

        }

    }

}

#endif /* SPHEREINITIALIZER_HPP_ */


// The MIT License
// (c) 2019 Daniel Williams

#include <cmath>
#include <cstdio>
#include <string>
#include <memory>
#include <iostream>
#include <iomanip>

using std::string;
using std::cerr;
using std::endl;

#include <Eigen/Geometry>
using Eigen::Vector2d;

#include <nlohmann/json/json.hpp>
using nlohmann::json;

#include "geometry/layout.hpp"
#include "quadtree/tree.hpp"

using namespace terrain;
using geometry::Layout;
using quadtree::Tree;
using quadtree::Node;

// main method for descending through a tree and returning the appropriate location / node / value
// note: weakly optimized; intended to be a hot path.
void descend( const Vector2d& target, double& x_c, double& y_c, const double start_width, Node* & current_node){
    double current_width = start_width;
    double next_width = start_width*0.5;

    while( ! current_node->is_leaf() )
    {
        current_width = next_width;
        next_width *= 0.5;

        if(target[0] > x_c){
            if( target[1] > y_c){
                x_c += next_width;
                y_c += next_width;
                current_node = current_node->get_northeast();
            }else{
                x_c += next_width;
                y_c -= next_width;
                current_node = current_node->get_southeast();
            }
        }else{
            if( target[1] > y_c){
                x_c -= next_width;
                y_c += next_width;
                current_node = current_node->get_northwest();
            }else{
                x_c -= next_width;
                y_c -= next_width;
                current_node = current_node->get_southwest();
            }
        }
    }
}

Tree::Tree(): Tree(Layout()) {}

Tree::Tree(const Layout& _layout)
    : layout(_layout)
{
    reset();
}

Tree::~Tree(){
    root.release();
}

bool Tree::contains(const Eigen::Vector2d& p) const {
    return layout.contains(p);
}

cell_value_t Tree::classify(const Eigen::Vector2d& p) const {
    // create a R/W copy, initialized at the tree's center.
    Eigen::Vector2d located( layout.get_center() );

    auto current_node = root.get();

    descend( p, located[0], located[1], layout.get_width(), current_node );

    return current_node->get_value();
}

void Tree::debug_tree(const bool show_pointers) const {
    cerr << "====== Quad Tree: ======\n";
    cerr << "##  bounds:     " << layout.to_string() << endl;
    cerr << "##  height:     " << get_height() << endl;
    cerr << "##  precision:  " << layout.get_precision() << endl;

    root->draw(cerr, "    ", "RT", show_pointers);
    cerr << endl;
}

size_t Tree::calculate_complete_tree(const size_t height){
    // see: https://en.wikipedia.org/wiki/M-ary_tree
    //      # properties of M-ary trees
    //
    //    m == branching_factor == 4  ///< for a quadtree, this is trivially 4
    //
    //       (h+1)
    //     m        - 1
    // N = ---------------
    //        m - 1
    //
    return ( pow(4,height+1) - 1 )/3;
}

double Tree::get_load_factor() const {
    const size_t height = root->get_height();
    const size_t count = root->get_count();
    const size_t complete = calculate_complete_tree(height);
    return static_cast<double>(count) / static_cast<double>(complete);
}

size_t Tree::get_memory_usage() const {
    return size() * sizeof(Node);
}

cell_value_t Tree::interp(const Eigen::Vector2d& at) const {

    // cout << "@@" << at << "    near: " << near.get_bounds() << " = " << near.get_value() << endl;

    // if Eigen::Vector2d is outside the tree, entirely
    if( ! contains(at)){
        return cell_default_value;
    }

    // const Node& near = root->search(at, get_bounds());
//     const Eigen::Vector2d& cn = near.get_center();
//     const double dx = std::copysign(1.0, (at.x() - cn.x())) * 2 * near.get_bounds().half_width;
//     const double dy = std::copysign(1.0, (at.y() - cn.y())) * 2 * near.get_bounds().half_width;
//     const Node& n2 = root->search({cn.x() + dx, cn.y()     }, get_bounds());
//     const Node& n3 = root->search({cn.x() + dx, cn.y() + dy}, get_bounds());
//     const Node& n4 = root->search({cn.x()     , cn.y() + dy}, get_bounds());

//     const auto& interp = near.interpolate_bilinear(at, n2, n3, n4);
    // return interp;

    return NAN;
}

void Tree::fill(const cell_value_t fill_value){
    root->fill(fill_value);
}

size_t Tree::get_height() const {
    return root->get_height() - 1;
}

bool Tree::load_tree(const nlohmann::json& doc){
    if(! doc.is_object()){
        cerr << "?? attempted to load unexpected format: no-object json document!\n";
        return false;
    }
    return root->load(doc);
}

void Tree::prune(){
    root->prune();
}

void Tree::reset(){
    root = std::make_unique<Node>(0);
}

void Tree::reset(const Layout& new_layout){
    layout = new_layout;

    root = std::make_unique<Node>(0);
    root->split(layout.get_precision(), layout.get_width());
}

Sample Tree::sample(const Eigen::Vector2d& p) const {
    Vector2d located( layout.get_center() );
    auto current_node = root.get();

    descend( p, located[0], located[1], layout.get_width(), current_node );

    return {located, current_node->get_value()};
}

bool Tree::store(const Vector2d& p, const cell_value_t new_value) {
    Vector2d located( layout.get_center() );
    auto current_node = root.get();

    descend( p, located[0], located[1], layout.get_width(), current_node );

    current_node->set_value(new_value);
    return true;
}

size_t Tree::size() const {
    return root->get_count();
}

json Tree::to_json_tree() const {
    return root->to_json();
}


#ifndef QSIM_APPLY_PAULI_HPP_
#define QSIM_APPLY_PAULI_HPP_

#include <armadillo>

void apply_pauli(int k, bool x, bool z, arma::cx_dvec &psi);
void apply_pauli_slow(int k, bool x, bool z, arma::cx_dvec &psi);
void apply_ppr(unsigned int x, unsigned int z, double theta, arma::cx_dvec &psi);
void apply_ppr_slow(unsigned int x, unsigned int z, double theta, arma::cx_dvec &psi);
double measure_pp(unsigned int x, unsigned int z, arma::cx_dvec &psi);
double measure_pp_slow(unsigned int x, unsigned int z, arma::cx_dvec & psi);

#endif // QSIM_APPLY_PAULI_HPP_