// Copyright 2020 Petr Petrovich Petrov. All rights reserved.
// License: https://github.com/PetrPPetrov/gkm-ship-cad/blob/master/LICENSE

#include <cassert>
#include <cstdint>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <string>
#include <sstream>
#include <vector>
#include <boost/polygon/polygon.hpp>
#include <QPainter>
#include <QApplication>
#include <QStatusBar>
#include <QOpenGLShader>
#include "main_window.h"
#include "view_3d_widget.h"

#define PROGRAM_VERTEX_ATTRIBUTE 0
#define PROGRAM_TEXCOORD_ATTRIBUTE 1

constexpr size_t VERTEX_COUNT = 1000000;

View3DWidget::View3DWidget(QWidget *parent) : QOpenGLWidget(parent)
{
    setMouseTracking(true);
    setDefaultCamera();
}

void View3DWidget::initializeGL()
{
    initializeOpenGLFunctions();
    glClearColor(0.5, 0.5, 0.5, 1.0);
    glEnable(GL_DEPTH_TEST);
    glDisable(GL_CULL_FACE);

    Gkm::Solid::ISolid::Ptr solid = g_main_window->getSolid();
    model = buildModel(solid);

    vbo.create();
    vbo.bind();
    vbo.allocate(&model->points[0], model->points.size() * sizeof(Eigen::Vector3f));

    QOpenGLShader* vshader = new QOpenGLShader(QOpenGLShader::Vertex, this);
    const char* vsrc =
        "attribute highp vec4 vertex;\n"
        "uniform mediump mat4 matrix;\n"
        "void main(void)\n"
        "{\n"
        "    gl_Position = matrix * vertex;\n"
        "}\n";
    vshader->compileSourceCode(vsrc);

    QOpenGLShader* fshader = new QOpenGLShader(QOpenGLShader::Fragment, this);
    const char* fsrc =
        "void main(void)\n"
        "{\n"
        "    gl_FragColor = vec4(1.0, 0.0, 0.0, 1.0);\n"
        "}\n";
    fshader->compileSourceCode(fsrc);

    program = std::make_unique<QOpenGLShaderProgram>();
    program->addShader(vshader);
    program->addShader(fshader);
    program->bindAttributeLocation("vertex", PROGRAM_VERTEX_ATTRIBUTE);
    program->link();

    program->bind();
}

void View3DWidget::paintGL()
{
    glClearColor(0.5, 0.5, 0.5, 1.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    QMatrix4x4 projection_matrix;
    projection_matrix.perspective(50.0f, static_cast<float>(width()) / height(), 0.125f, 1024.0f);

    QMatrix4x4 view_matrix;
    view_matrix.lookAt(viewer_pos, viewer_target, viewer_up);

    program->setUniformValue("matrix", projection_matrix * view_matrix);
    program->enableAttributeArray(PROGRAM_VERTEX_ATTRIBUTE);
    program->setAttributeBuffer(PROGRAM_VERTEX_ATTRIBUTE, GL_FLOAT, 0, 3, 3 * sizeof(GLfloat));

    glDrawArrays(GL_TRIANGLES, 0, model->points.size() / 3);
}

void View3DWidget::mouseMoveEvent(QMouseEvent* event)
{
    if (right_mouse_pressed)
    {
        // Pan mode
        QPointF current_point = event->localPos();
        double delta_x = (previous_point.x() - current_point.x()) / width() * rotation_radius;
        double delta_y = (current_point.y() - previous_point.y()) / height() * rotation_radius;
        auto user_position = viewer_previous_pos - viewer_previous_target;
        auto right = QVector3D::crossProduct(viewer_up, user_position).normalized();
        auto offset = right * delta_x + viewer_up * delta_y;
        viewer_pos = viewer_previous_pos + offset;
        viewer_target = viewer_previous_target + offset;
        // Restore rotation orbit radius
        viewer_target = viewer_pos + (viewer_target - viewer_pos).normalized() * rotation_radius;
        update();
    }
    else if (left_mouse_pressed)
    {
        // Rotation mode
        QPointF current_point = event->localPos();
        double delta_x = previous_point.x() - current_point.x();
        double delta_y = current_point.y() - previous_point.y();
        double x_rotation_angle = delta_x / width() * 180.0f;
        double y_rotation_angle = delta_y / height() * 180.0f;
        auto user_position = viewer_previous_pos - viewer_previous_target;
        QMatrix4x4 rotation_x;
        rotation_x.rotate(x_rotation_angle, viewer_previous_up);
        auto temp_user_position = rotation_x * user_position;
        auto left = QVector3D::crossProduct(temp_user_position, viewer_previous_up).normalized();
        QMatrix4x4 rotation_y;
        rotation_y.rotate(y_rotation_angle, left);
        auto result_rotation = rotation_y * rotation_x;
        auto rotated_user_position = (result_rotation * user_position).normalized() * rotation_radius;
        viewer_pos = viewer_previous_target + rotated_user_position;
        viewer_up = (result_rotation * viewer_previous_up).normalized();
        // Restore up vector property: up vector must be orthogonal to direction vector
        auto new_left = QVector3D::crossProduct(rotated_user_position, viewer_up);
        viewer_up = QVector3D::crossProduct(new_left, rotated_user_position).normalized();
        update();
    }
}

void View3DWidget::mousePressEvent(QMouseEvent* event)
{
    bool left_or_right = false;
    if (event->buttons() & Qt::LeftButton)
    {
        left_mouse_pressed = true;
        left_or_right = true;
    }
    if (event->buttons() & Qt::RightButton)
    {
        right_mouse_pressed = true;
        left_or_right = true;
    }
    if (left_or_right)
    {
        previous_point = event->localPos();
        viewer_previous_pos = viewer_pos;
        viewer_previous_target = viewer_target;
        viewer_previous_up = viewer_up;
    }
    update();
}

void View3DWidget::mouseReleaseEvent(QMouseEvent* event)
{
    bool left_or_right = false;
    if (!(event->buttons() & Qt::LeftButton))
    {
        left_mouse_pressed = false;
        left_or_right = true;
    }
    if (!(event->buttons() & Qt::RightButton))
    {
        right_mouse_pressed = false;
        left_or_right = true;
    }
    if (left_or_right)
    {
        previous_point = event->localPos();
        viewer_previous_pos = viewer_pos;
        viewer_previous_target = viewer_target;
        viewer_previous_up = viewer_up;
    }
    update();
}

void View3DWidget::wheelEvent(QWheelEvent* event)
{
    QPoint delta = event->angleDelta();
    rotation_radius += delta.y() / 1000.0f * rotation_radius;
    if (rotation_radius < minimum_rotation_radius)
    {
        rotation_radius = minimum_rotation_radius;
    }
    if (rotation_radius > maximum_rotation_radius)
    {
        rotation_radius = maximum_rotation_radius;
    }
    auto user_position = viewer_pos - viewer_target;
    auto new_user_position = user_position.normalized() * rotation_radius;
    viewer_pos = viewer_target + new_user_position;
    update();
}

void View3DWidget::setDefaultCamera()
{
    viewer_pos = QVector3D(0, 0, 3);
    viewer_target = QVector3D(0, 0, 0);
    viewer_up = QVector3D(0, 1, 0);
    rotation_radius = viewer_pos.distanceToPoint(viewer_target);
    viewer_previous_pos = viewer_pos;
    viewer_previous_target = viewer_target;
    viewer_previous_up = viewer_up;
    minimum_rotation_radius = 0.1;
    maximum_rotation_radius = 1000.0;
}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/tools/minima.hpp>
#include <vinecopulib/misc/tools_bobyqa.hpp>

namespace vinecopulib {

//! Utilities for numerical optimization (based on Bobyqa)
namespace tools_optimization {

//! creates an Optimizer using the default controls, see BobyqaControls.
//!
//! @param n_parameters Number of parameters to optimize
//! @param lower_bounds
//! @param upper_bounds
//! @param objective The optimizer's objective function
inline Optimizer::Optimizer()
  : controls_(BobyqaControls())
{}

//! Sets the optimizer's controls..
//!
//! @param initial_trust_region Initial trust region.
//! @param final_trust_region Final trust region.
//! @param maxeval Maximal number of evaluations of the objective.
inline void
Optimizer::set_controls(double initial_trust_region,
                        double final_trust_region,
                        int maxeval)
{
  controls_ = BobyqaControls(initial_trust_region, final_trust_region, maxeval);
}

//! @brief Solve the maximization problem.
//!
//! @param initial_parameters Starting values for the optimization
//!     algorithm.
//! @param lower_bounds Lower bounds for the parameters.
//! @param upper_bounds Upper bounds for the parameters.
//! @param the Objective function to maximize.
//! @return the optimal parameters.
inline Eigen::VectorXd
Optimizer::optimize(const Eigen::VectorXd& initial_parameters,
                    const Eigen::VectorXd& lower_bounds,
                    const Eigen::VectorXd& upper_bounds,
                    std::function<double(const Eigen::VectorXd&)> objective)
{
  check_parameters_size(initial_parameters, lower_bounds, upper_bounds);
  size_t n_parameters = initial_parameters.size();
  auto optimal_parameters = initial_parameters;
  if (n_parameters > 1) {
    // const int number_interpolation_conditions = (n_parameters + 1) *
    //        (n_parameters + 2)/2;
    size_t number_interpolation_conditions = n_parameters + 3;
    std::function<double(size_t, const double*)> f =
      [objective, this](size_t n, const double* x) {
        Eigen::Map<const Eigen::VectorXd> par(x, n);
        this->objective_calls_++;
        return -objective(par);
      };
    auto result = tools_bobyqa::bobyqa(f,
                                       n_parameters,
                                       number_interpolation_conditions,
                                       initial_parameters,
                                       lower_bounds,
                                       upper_bounds,
                                       controls_.get_initial_trust_region(),
                                       controls_.get_final_trust_region(),
                                       controls_.get_maxeval());
    optimal_parameters = result.first;
    objective_max_ = -result.second;
  } else {
    double eps = 1e-6;
    std::function<double(double)> f = [objective, this](double x) {
      Eigen::Map<const Eigen::VectorXd> par(&x, 1);
      this->objective_calls_++;
      return -objective(par);
    };
    auto result = boost::math::tools::brent_find_minima(
      f,
      static_cast<double>(lower_bounds(0)) + eps,
      static_cast<double>(upper_bounds(0)) - eps,
      20);
    optimal_parameters(0) = result.first;
    objective_max_ = -result.second;
  }

  return optimal_parameters;
}

//! @brief Returns how often the objective function was called.
inline size_t
Optimizer::get_objective_calls() const
{
  return objective_calls_;
}

//! @brief Returns the objective value at the maximum.
inline double
Optimizer::get_objective_max() const
{
  return objective_max_;
}

//! checks whether sizes of parameters and bounds match.
inline void
Optimizer::check_parameters_size(const Eigen::VectorXd& initial_parameters,
                                 const Eigen::VectorXd& lower_bounds,
                                 const Eigen::VectorXd& upper_bounds) const
{
  if (initial_parameters.size() != upper_bounds.size()) {
    throw std::runtime_error(
      "initial parameters and and bounds must have same size.");
  }
  if (lower_bounds.size() != upper_bounds.size()) {
    throw std::runtime_error("lower and upper bounds must have same size.");
  }
  if (lower_bounds.size() < 1) {
    throw std::runtime_error("n_parameters should be larger than 0.");
  }
}

//! Instantiates controls using the default contructor
//!
//! The defaults are
//! ```
//! initial_trust_region_ = 1e-4;
//! final_trust_region_ = 1e3;
//! maxeval_ = 1000;
//! ```
inline BobyqaControls::BobyqaControls()
{
  initial_trust_region_ = 1e-4;
  final_trust_region_ = 1e3;
  maxeval_ = 1000;
}

//! Instantiates controls by passing the arguments
//!
//! @param initial_trust_region Initial trust region.
//! @param final_trust_region Final trust region.
//! @param maxeval Maximal number of evaluations of the objective.
inline BobyqaControls::BobyqaControls(double initial_trust_region,
                                      double final_trust_region,
                                      int maxeval)
{
  check_parameters(initial_trust_region, final_trust_region, maxeval);
  initial_trust_region_ = initial_trust_region;
  final_trust_region_ = final_trust_region;
  maxeval_ = maxeval;
}

inline void
BobyqaControls::check_parameters(double initial_trust_region,
                                 double final_trust_region,
                                 int maxeval)
{
  if (initial_trust_region <= 0) {
    throw std::runtime_error("initial_trust_region should be larger than 0");
  }
  if (final_trust_region <= 0) {
    throw std::runtime_error("final_trust_region should be larger than 0");
  }
  if (maxeval <= 0) {
    throw std::runtime_error("maxeval should be larger than 0");
  }
}

//! @name Getters and setters
//! @{

//! @return the initial trust region.
inline double
BobyqaControls::get_initial_trust_region()
{
  return initial_trust_region_;
}

//! @return the final trust region.
inline double
BobyqaControls::get_final_trust_region()
{
  return final_trust_region_;
}

//! @return the maximal number of evaluations of the objective.
inline int
BobyqaControls::get_maxeval()
{
  return maxeval_;
}

//! @}
}
}


//
// file BasePPhoreSite.cc
// David Cosgrove
// AstraZeneca
// 10th October 2006
//
// Implementation of BasePPhoreSite.

#include <algorithm>
#include <numeric>

#include <boost/bind.hpp>

#include "stddefs.H"
#include "BasePPhoreSite.H"
#include "SinglePPhoreSite.H" // bad karma!
#include "OverlayTrans.H"

namespace DACLIB {
void rotate_about_axis( const double axis[3] , const double angle ,
double vec[3] );

double angle_about_axis( const double axis[3] , const double vec1[3] ,
const double vec2[3] );
float torsion( const float *cds1 , const float *cds2 , const float *cds3 ,
               const float *cds4 , bool degs );
}

using namespace std;

// *******************************************************************************
BasePPhoreSite::BasePPhoreSite() {

  reset_data();

}

// *******************************************************************************
// tc and ts are the type code and type string respectively.
BasePPhoreSite::BasePPhoreSite( const double cds[3] , const double dir[3] , int tc ,
const string &ts , const string &lbl , bool use_dir ) {

  label_ = lbl;
  num_dirs_ = use_dir ? 1 : 0;
  selected_ = false;

  type_code_ = tc;
  type_string_ = ts;

  cds_[0] = cds[0]; cds_[1] = cds[1]; cds_[2] = cds[2];

  if( use_dir ) {
    dir_[0] = dir[0]; dir_[1] = dir[1]; dir_[2] = dir[2];
  } else {
    dir_[0] = dir_[1] = dir_[2] = 0.0;
  }

  dir_moves_ = GtplDefs::NONE;

  num_virt_sites_ = 0;
  active_virt_site_ = -1;

}

// *******************************************************************************
BasePPhoreSite::~BasePPhoreSite() {

}

// ****************************************************************************
void BasePPhoreSite::reset_data() {

  label_ = "";

  num_dirs_ = 0;
  selected_ = false;

  type_code_ = -1;
  type_string_ = "";

  cds_[0] = cds_[1] = cds_[2] = 0.0;
  dir_[0] = dir_[1] = dir_[2] = 0.0;
  dir_moves_ = GtplDefs::NONE;

  num_virt_sites_ = 0;
  active_virt_site_ = -1;

}

// ****************************************************************************
void BasePPhoreSite::copy_data( const BasePPhoreSite &c ) {

  label_ = c.label_;

  type_code_ = c.type_code_;
  type_string_ = c.type_string_;

  cds_[0] = c.cds_[0]; cds_[1] = c.cds_[1]; cds_[2] = c.cds_[2];

  num_dirs_ = c.num_dirs_;
  if( c.num_dirs_ ) {
    copy( c.dir_ , c.dir_ + num_dirs_ * 3 , dir_ );
    copy( c.dir_types_ , c.dir_types_ + num_dirs_ , dir_types_ );
  }

  selected_ = c.selected_;

  dir_moves_ = c.dir_moves_;
  twiddle_axis_[0] = c.twiddle_axis_[0];
  twiddle_axis_[1] = c.twiddle_axis_[1];
  twiddle_axis_[2] = c.twiddle_axis_[2];

  num_virt_sites_ = c.num_virt_sites_;
  copy( c.virt_sites_ , c.virt_sites_ + 3 * num_virt_sites_ , virt_sites_ );
  active_virt_site_ = c.active_virt_site_;

}

// ****************************************************************************
void BasePPhoreSite::set_coords( const double *new_cds ) {

  cds_[0] = new_cds[0];
  cds_[1] = new_cds[1];
  cds_[2] = new_cds[2];

}

// ****************************************************************************
void BasePPhoreSite::set_coords( const float *new_cds ) {

  cds_[0] = new_cds[0];
  cds_[1] = new_cds[1];
  cds_[2] = new_cds[2];

}

// ****************************************************************************
int BasePPhoreSite::get_num_dirs( GtplDefs::DIRS_TYPE dirs_type ) const {

  int ret_num = 0;
  for( int i = 0 ; i < num_dirs_ ; ++i )
    if( dirs_type == dir_types_[i] )
      ++ret_num;

  return ret_num;

}

// ****************************************************************************
const double *BasePPhoreSite::direction( int dir_num ) const {

  if( dir_num < 0 || dir_num >= num_dirs_ )
    return 0;
  else
    return dir_ + 3 * dir_num;

}

// ****************************************************************************
GtplDefs::DIRS_TYPE BasePPhoreSite::direction_type( int dir_num ) const {

  if( dir_num < 0 || dir_num >= num_dirs_ )
    return GtplDefs::UNKNOWN;
  else
    return dir_types_[dir_num];

}

// ****************************************************************************
void BasePPhoreSite::set_direction( const double *new_dir ,
                                    GtplDefs::DIRS_TYPE t , int dir_num  ) {

  if( -1 == dir_num && num_dirs_ < 3 ) {
    dir_[3 * num_dirs_] = new_dir[0];
    dir_[3 * num_dirs_ + 1] = new_dir[1];
    dir_[3 * num_dirs_ + 2] = new_dir[2];
    dir_types_[num_dirs_] = t;
    ++num_dirs_;
  }
  if( -1 != dir_num && dir_num < 3 ) {
    dir_[3 * dir_num] = new_dir[0];
    dir_[3 * dir_num + 1] = new_dir[1];
    dir_[3 * dir_num + 2] = new_dir[2];
    dir_types_[dir_num] = t;
  }

}

// ****************************************************************************
void BasePPhoreSite::set_direction( const float *new_dir ,
                                    GtplDefs::DIRS_TYPE t , int dir_num ) {

  if( -1 == dir_num && num_dirs_ < 3 ) {
    dir_[3 * num_dirs_] = new_dir[0];
    dir_[3 * num_dirs_ + 1] = new_dir[1];
    dir_[3 * num_dirs_ + 2] = new_dir[2];
    dir_types_[num_dirs_] = t;
    ++num_dirs_;
  }
  if( -1 != dir_num && dir_num < 3 ) {
    dir_[3 * dir_num] = new_dir[0];
    dir_[3 * dir_num + 1] = new_dir[1];
    dir_[3 * dir_num + 2] = new_dir[2];
    dir_types_[dir_num] = t;
  }

}

// ****************************************************************************
float BasePPhoreSite::square_distance( const double *cds ) const {

  return ( DACLIB::square( cds[0] - cds_[0] ) +
      DACLIB::square( cds[1] - cds_[1] ) +
      DACLIB::square( cds[2] - cds_[2] ) );

}

// ****************************************************************************
void BasePPhoreSite::translate( float x_trans , float y_trans , float z_trans ) {

  cds_[0] += x_trans;
  cds_[1] += y_trans;
  cds_[2] += z_trans;

}

// ****************************************************************************
void BasePPhoreSite::rotate( const float rot[3][3] ) {

  float cds[3];
  cds[0] = rot[0][0] * cds_[0] + rot[0][1] * cds_[1] + rot[0][2] * cds_[2];
  cds[1] = rot[1][0] * cds_[0] + rot[1][1] * cds_[1] + rot[1][2] * cds_[2];
  cds[2] = rot[2][0] * cds_[0] + rot[2][1] * cds_[1] + rot[2][2] * cds_[2];

  cds_[0] = cds[0];
  cds_[1] = cds[1];
  cds_[2] = cds[2];

  cds[0] = rot[0][0] * twiddle_axis_[0] + rot[0][1] * twiddle_axis_[1] + rot[0][2] * twiddle_axis_[2];
  cds[1] = rot[1][0] * twiddle_axis_[0] + rot[1][1] * twiddle_axis_[1] + rot[1][2] * twiddle_axis_[2];
  cds[2] = rot[2][0] * twiddle_axis_[0] + rot[2][1] * twiddle_axis_[1] + rot[2][2] * twiddle_axis_[2];

  twiddle_axis_[0] = cds[0];
  twiddle_axis_[1] = cds[1];
  twiddle_axis_[2] = cds[2];

  for( int i = 0 ; i < num_dirs_ ; ++i ) {
    cds[0] =
        rot[0][0] * dir_[3*i] + rot[0][1] * dir_[3*i+1] + rot[0][2] * dir_[3*i+2];
    cds[1] =
        rot[1][0] * dir_[3*i] + rot[1][1] * dir_[3*i+1] + rot[1][2] * dir_[3*i+2];
    cds[2] =
        rot[2][0] * dir_[3*i] + rot[2][1] * dir_[3*i+1] + rot[2][2] * dir_[3*i+2];

    dir_[3*i] = cds[0];
    dir_[3*i+1] = cds[1];
    dir_[3*i+2] = cds[2];
  }

  for( int i = 0 ; i < num_virt_sites_ ; ++i ) {
    cds[0] =
        rot[0][0] * virt_sites_[3*i] + rot[0][1] * virt_sites_[3*i+1] + rot[0][2] * virt_sites_[3*i+2];
    cds[1] =
        rot[1][0] * virt_sites_[3*i] + rot[1][1] * virt_sites_[3*i+1] + rot[1][2] * virt_sites_[3*i+2];
    cds[2] =
        rot[2][0] * virt_sites_[3*i] + rot[2][1] * virt_sites_[3*i+1] + rot[2][2] * virt_sites_[3*i+2];

    virt_sites_[3*i] = cds[0];
    virt_sites_[3*i+1] = cds[1];
    virt_sites_[3*i+2] = cds[2];
  }

}

// ****************************************************************************
// find the order of directions in site that has the most of the given
// type lined up with directions in this. Dir_order maps dirs_ vectors in
// site onto dirs_ vectors in this. Returns the sum of the dot-products for
// the order found.
float BasePPhoreSite::best_dir_alignments( BasePPhoreSite &site ,
                                           GtplDefs::DIRS_TYPE dirs_type ,
                                           bool twiddle_if_poss ,
                                           vector<int> &dir_order ) {

  // needs to be done with num_dirs_ >= site.num_dirs_
  if( site.num_dirs_ > num_dirs_ ) {
    return site.best_dir_alignments( *this , dirs_type , twiddle_if_poss ,
                                     dir_order );
  }
  vector<int> combs;
  for( int i = 0 ; i < site.num_dirs_ ; ++i ) {
    combs.push_back( i );
  }

  float best_dot = -1.0;
  dir_order = combs;
  do {
#ifdef NOTYET
    cout << "Next comb : ";
    copy( combs.begin() , combs.end() , ostream_iterator<int>( cout , " " ) );
    cout << " :: " << dirs_type << endl;
#endif
    // line up on the first direction if appropriate
    if( twiddle_if_poss && site.get_twiddlable() ) {
      site.twiddle( dir_ , combs[0] );
    }

    float cum_dot = 0.0F;
    for( int i = 0 ; i < site.num_dirs_ ; ++i ) {
      if( dir_types_[i] == dirs_type )
        cum_dot = DACLIB::dot_product( dir_ + 3 * i , site.dir_ + 3 * combs[i] );
    }
    if( cum_dot > best_dot ) {
      best_dot = cum_dot;
      dir_order = combs;
    }
  } while ( next_permutation( combs.begin() , combs.end() ) );

  return best_dot;

}

// ****************************************************************************
// put the direction vectors into the new order.
void BasePPhoreSite::reorder_dirs( const vector<int> &new_order ) {

  double dirs2[9];
  GtplDefs::DIRS_TYPE dt[3];
#ifdef NOTYET
  cout << "new orders : " << new_order.size() << " :: ";
  copy( new_order.begin() , new_order.end() , intOut );
  cout << endl;
  cout << "num_dirs : " << num_dirs_ << " : " << get_num_dirs() << endl;
#endif

  for( int i = 0 ; i < std::min( num_dirs_ , int( new_order.size() ) ) ; ++i ) {
    dirs2[3 * i] = dir_[3 * new_order[i]];
    dirs2[3 * i + 1] = dir_[3 * new_order[i] + 1];
    dirs2[3 * i + 2] = dir_[3 * new_order[i] + 2];
    dt[i] = dir_types_[new_order[i]];
  }

  for( int i = 0 ; i < get_num_dirs() ; ++i ) {
    dir_[3 * i] = dirs2[3 * i];
    dir_[3 * i + 1] = dirs2[3 * i + 1];
    dir_[3 * i + 2] = dirs2[3 * i + 2];
    dir_types_[i] = dt[i];
  }

}

// ****************************************************************************
// twiddle the given direction to point along the given vector dir
// if it's twiddlable leaving it unchanged if it isn't
void BasePPhoreSite::twiddle( const double *dir , int dir_num ) {

  if( dir_moves_ != GtplDefs::FREE || dir_num < 0 || dir_num >= num_dirs_ ) {
    return;
  }

  double in_dir[3] = { dir[0] , dir[1] , dir[2] };
  double this_dir[3] = { dir_[3 * dir_num] , dir_[3 * dir_num + 1] ,
                         dir_[3 * dir_num + 2] };

  double angle = DACLIB::angle_about_axis( twiddle_axis_ , in_dir , this_dir );
  DACLIB::rotate_about_axis( twiddle_axis_ , -angle , this_dir );
  double angle_to_use = -angle;
  // sometimes the angle comes back with the wrong sign, for reasons which
  // I don't understand and haven't delved into deeply. If it's right, the
  // rotation will put the vectors with essentially a zero angle.  If not, try
  // it the other way.
  double new_angle = DACLIB::angle_about_axis( twiddle_axis_ , in_dir , this_dir );
  if( new_angle > 1.0e-6 ) {
    double othis_dir[3] = { dir_[3 * dir_num] , dir_[3 * dir_num + 1] ,
                           dir_[3 * dir_num + 2] };
    DACLIB::rotate_about_axis( twiddle_axis_ , angle , this_dir );
    double rnew_angle = DACLIB::angle_about_axis( twiddle_axis_ , in_dir , othis_dir );
    if( rnew_angle < new_angle ) {
      angle_to_use = angle;
    }
  }

  for( int i = 0 ; i < num_dirs_ ; ++i ) {
    DACLIB::rotate_about_axis( twiddle_axis_ , angle_to_use , dir_ + 3 * i );
  }

}

// ****************************************************************************
// this one aligns all directions of given type as best as possible, and
// rearranges the directions to match those in site.
void BasePPhoreSite::twiddle( BasePPhoreSite &site ,
                              GtplDefs::DIRS_TYPE dirs ) {

  if( dir_moves_ != GtplDefs::FREE || !num_dirs_ || !site.num_dirs_ ) {
    return;
  }

  // needs to be done with num_dirs_ >= site.num_dirs_
  if( site.num_dirs_ > num_dirs_ ) {
    site.twiddle( *this , dirs );
    return;
  }

  vector<int> best_comb;
  best_dir_alignments( site , dirs , true , best_comb );

  // make best_comb the directions
  site.twiddle( dir_ , best_comb[0] );

  site.reorder_dirs( best_comb );

}

// ****************************************************************************
// flip directions by 180
void BasePPhoreSite::flip() {

  if( dir_moves_ != GtplDefs::FLIP )
    return;

  for( int i = 0 ; i < num_dirs_ ; ++i )
    DACLIB::rotate_about_axis( twiddle_axis_ , M_PI , dir_ + 3 * i );

}

// ****************************************************************************
// flip directions by 180 if the alignments between dirs_ of type dirs
// gives a better score that way.
void BasePPhoreSite::flip( BasePPhoreSite &site , GtplDefs::DIRS_TYPE dirs ) {

  if( dir_moves_ != GtplDefs::FLIP )
    return;

  vector<int> score1_comb;
  float score1 = best_dir_alignments( site , dirs , false , score1_comb );

  flip();
  vector<int> score2_comb;
  float score2 = best_dir_alignments( site , dirs , false , score2_comb );

  if( score1 > score2 ) {
    // put it back again
    site.reorder_dirs( score1_comb );
    flip();
  } else {
    site.reorder_dirs( score2_comb );
  }

}

// ****************************************************************************
void BasePPhoreSite::set_virt_sites( const double *vs, int num_vs ) {

  num_virt_sites_ = num_vs;
  copy( vs , vs + 3 * num_vs , virt_sites_ );

}

// ****************************************************************************
void BasePPhoreSite::write_to_stream( ostream &os ) const {

  os << label_ << endl << type_code_ << endl << type_string_ << endl
     << cds_[0] << " " << cds_[1] << " " << cds_[2] << endl
     << num_dirs_ << endl;
  for( int i = 0 ; i < num_dirs_ ; ++i )
    os << dir_[3*i] << " " << dir_[3*i+1] << " " << dir_[3*i+2]
                    << " " << dir_types_[i] << endl;

  os << dir_moves_ << endl;
  if( dir_moves_ != GtplDefs::NONE )
    os << twiddle_axis_[0] << " " << twiddle_axis_[1] << " "
                           << twiddle_axis_[2] << endl;

}

// ****************************************************************************
void BasePPhoreSite::read_from_stream( istream &is ) {

  is >> label_ >> type_code_ >> type_string_
      >> cds_[0] >> cds_[1] >> cds_[2] >> num_dirs_;
  for( int i = 0 ; i < num_dirs_ ; ++i ) {
    is >> dir_[3*i] >> dir_[3*i+1] >> dir_[3*i+2];
    int j;
    is >> j;
    dir_types_[i] = (GtplDefs::DIRS_TYPE) j;
  }

  int i;
  is >> i;
  dir_moves_ = (GtplDefs::DIR_MOVES) i;
  if( dir_moves_ != GtplDefs::NONE )
    is >> twiddle_axis_[0] >> twiddle_axis_[1] >> twiddle_axis_[2];

}

// ****************************************************************************
void BasePPhoreSite::brief_report( ostream &os ) const {

  os << "Site " << get_full_name() << " :: ";
  os << "(" << cds_[0] << " , " << cds_[1] << " , " << cds_[2] << ")";
  for( int j = 0 , js = num_dirs_ ; j < js ; ++j ) {
    os << " (" << dir_[3 * j + 0] << " , " << dir_[3 * j + 1]
       << " , " << dir_[3 * j + 2] << ")";
  }
  os << endl;

}

// ****************************************************************************
void brief_report_sites( ostream &os , const vector<BasePPhoreSite *> &sites ) {

  for( unsigned int i = 0 ; i < sites.size() ; i++ ) {
    sites[i]->brief_report( os );
  }

}

// ****************************************************************************
// calculate the overlay transformation to move the given pairs of the second
// vector of PPhoreSites onto the first, returning the RMS of the overlay, but
// not moving the sites.
float calc_overlay_trans( const vector<BasePPhoreSite *> &sites1 ,
                          const vector<BasePPhoreSite *> &sites2 ,
                          const vector<int> &pairs ,
                          OverlayTrans &overlay_trans , bool use_ring_norm_dirs ,
                          bool use_h_vec_dirs , bool use_lp_dirs ) {

  vector<float> cds1 , cds2;

  for( unsigned int i = 0 , is = pairs.size() ; i < is ; i += 2 ) {
    const double *sds1 = sites1[pairs[i]]->coords();
    const double *sds2 = sites2[pairs[i+1]]->coords();
    cds1.insert( cds1.end() , sds1 , sds1 + 3 );
    cds2.insert( cds2.end() , sds2 , sds2 + 3 );
  }

  if( use_ring_norm_dirs ) {
    add_ring_norm_dir_sites( sites1 , sites2 , pairs , cds1 , cds2 );
  }

  if( use_h_vec_dirs ) {
    add_dir_sites( GtplDefs::H_VECTOR , sites1 , sites2 , pairs ,
                   cds1 , cds2 );
  }
  if( use_lp_dirs ) {
    add_dir_sites( GtplDefs::LP_VECTOR , sites1 , sites2 , pairs ,
                   cds1 , cds2 );
  }
  vector<float> cds1_cp( cds1 ) , cds2_cp( cds2 );

  // OverlayTrans c'tor moves first coords onto 2nd. It centres the coords, so
  // changes the originals, hence the need for cds1_cp and cds2_cp.
  int num_cds = cds2.size() / 3;

  overlay_trans = OverlayTrans( &cds2[0] , &cds1[0] , num_cds );
  // do the overlay so we can calculate the RMS.
  overlay_trans.overlay( num_cds , &cds2_cp[0] );

  // don't want to calculate RMS on the extension points as well
  int num_sites = pairs.size() / 2;
  float rms = inner_product( cds1_cp.begin() , cds1_cp.begin() + 3 * num_sites , cds2_cp.begin() ,
                             0.0F , plus<float>() ,
                             boost::bind( multiplies<float>() ,
                                          boost::bind( minus<float>() , _1 , _2 ) ,
                                          boost::bind( minus<float>() , _1 , _2 ) ) );

  rms = sqrt( rms / float( num_cds ) );

  return rms;

}

// **************************************************************************
void add_ring_norm_dir_sites( const vector<BasePPhoreSite *> &sites1 ,
                              const vector<BasePPhoreSite *> &sites2 ,
                              const vector<int> &pairs , vector<float> &cds1 ,
                              vector<float> &cds2 ) {

  float pcds1[3] , pcds2[3];

  for( int i = 0 , is = pairs.size() ; i < is ; i += 2 ) {
    BasePPhoreSite *site1 = sites1[pairs[i]];
    BasePPhoreSite *site2 = sites2[pairs[i+1]];
    if( site1->get_num_dirs() && site2->get_num_dirs() ) {
      // find the end of the normal for each site
      const double *dir1 = 0 , *dir2 = 0;
      int site1_dir_num = -1;
      for( int j = 0 ; j < 3 ; ++j ) {
        if( GtplDefs::RING_NORMAL == site1->direction_type( j ) ) {
          dir1 = site1->direction( j );
          site1_dir_num = j;
          break;
        }
      }
      if( !dir1 )
        continue;
      int site2_dir_num = -1;
      for( int j = 0 ; j < 3 ; ++j ) {
        if( GtplDefs::RING_NORMAL == site2->direction_type( j ) ) {
          dir2 = site2->direction( j );
          site2_dir_num = j;
          break;
        }
      }
      if( !dir2 )
        continue;
      double cdir2[3] = { dir2[0] , dir2[1] , dir2[2] };
      const double *sds1 = site1->coords();
      const double *sds2 = site2->coords();
      pcds1[0] = sds1[0] + dir1[0];
      pcds1[1] = sds1[1] + dir1[1];
      pcds1[2] = sds1[2] + dir1[2];
      // depending on arbitrary factors, ring normals might be pointing in opposite
      // directions. This is why this only works on overlaid sites (not that
      // we're checking this anywhere).
      if( DACLIB::dot_product( site1->direction( site1_dir_num ) ,
                               site2->direction( site2_dir_num ) ) <= 0.0 ) {
        cdir2[0] *= -1.0;
        cdir2[1] *= -1.0;
        cdir2[2] *= -1.0;
        site2->set_direction( cdir2 , site1->direction_type( site1_dir_num ) ,
                              site2_dir_num );
      }
      pcds2[0] = sds2[0] + cdir2[0];
      pcds2[1] = sds2[1] + cdir2[1];
      pcds2[2] = sds2[2] + cdir2[2];
      cds1.insert( cds1.end() , pcds1 , pcds1 + 3 );
      cds2.insert( cds2.end() , pcds2 , pcds2 + 3 );
    }
  }

}

// **************************************************************************
// return the directions of the given type
void get_site_dirs( GtplDefs::DIRS_TYPE dt , BasePPhoreSite &site ,
                    double dirs[9] , int &num_dirs ) {

  num_dirs = 0;
  for( int i = 0 , is = site.get_num_dirs() ; i < is ; ++i ) {
    if( dt == site.direction_type( i ) ) {
      dirs[num_dirs++] = site.direction( i )[0];
      dirs[num_dirs++] = site.direction( i )[1];
      dirs[num_dirs++] = site.direction( i )[2];
    }
  }

  num_dirs /= 3;

}

// **************************************************************************
void add_site_coords( const double *cds , const double *dir ,
                      vector<float> &site_cds ) {

  site_cds.push_back( cds[0] + dir[0] );
  site_cds.push_back( cds[1] + dir[1] );
  site_cds.push_back( cds[2] + dir[2] );

}

// **************************************************************************
// by convention, sites1 is fixed, sites2 is moving
void add_dir_sites( GtplDefs::DIRS_TYPE dirs_type ,
                    const vector<BasePPhoreSite *> &sites1 ,
                    const vector<BasePPhoreSite *> &sites2 ,
                    const vector<int> &pairs , vector<float> &cds1 ,
                    vector<float> &cds2 ) {

  // this is all a bit complicated, as there may be more than one vector per site
  // and the site itself may be twiddlable or flippable

  double site1_dirs[9] , site2_dirs[9];
  int nsd1 , nsd2;
  for( int i = 0 , is = pairs.size() ; i < is ; i += 2 ) {
    BasePPhoreSite *site1 , *site2;
    BasePPhoreSite *in_site1 = sites1[pairs[i]] , *in_site2 = sites2[pairs[i+1]];

    bool made_site1( false ) , made_site2( false );
    if( in_site1->get_twiddlable() || in_site1->get_flippable() ) {
      site1 = new SinglePPhoreSite;
      site1->set_flippable( in_site1->get_flippable() );
      site1->set_twiddlable( in_site1->get_twiddlable() );
      for( int j = 0 , js = in_site1->get_num_dirs() ; j < js ; ++j ) {
        site1->set_direction( in_site1->direction( j ) , in_site1->direction_type( j ) ,
                              -1 ); // -1 makes a new direction
      }
      site1->set_coords( in_site1->coords() );
      site1->set_twiddle_axis( in_site1->twiddle_axis() );
      made_site1 = true;
    } else {
      site1 = in_site1;
    }
    if( in_site2->get_twiddlable() || in_site2->get_flippable() ) {
      site2 = new SinglePPhoreSite;
      site2->set_flippable( in_site2->get_flippable() );
      site2->set_twiddlable( in_site2->get_twiddlable() );
      for( int j = 0 , js = in_site2->get_num_dirs() ; j < js ; ++j ) {
        site2->set_direction( in_site2->direction( j ) , in_site2->direction_type( j ) ,
                              -1 ); // -1 makes a new direction
      }
      site2->set_coords( in_site2->coords() );
      site2->set_twiddle_axis( in_site2->twiddle_axis() );
      made_site2 = true;
    } else {
      site2 = in_site2;
    }

#ifdef NOTYET
    cout << "Site 1 " << site1->label() << " : " << site1->get_num_dirs() << " and "
         << "Site 2 " << site2->label() << " : " << site2->get_num_dirs() << endl;
#endif

    get_site_dirs( dirs_type , *site1 , site1_dirs , nsd1 );
    if( !nsd1 ) {
      if( made_site1 ) {
        delete site1;
      }
      if( made_site2 ) {
        delete site2;
      }
      continue;
    }
    get_site_dirs( dirs_type , *site2 , site2_dirs , nsd2 );
    if( !nsd2 ) {
      if( made_site1 ) {
        delete site1;
      }
      if( made_site2 ) {
        delete site2;
      }
      continue;
    }

    if( site2->get_twiddlable() ) {
      site2->twiddle( *site1 , dirs_type );
    } else if( site1->get_twiddlable() ) {
      site1->twiddle( *site2 , dirs_type );
    }
    if( site2->get_flippable() ) {
      site2->flip( *site1 , dirs_type );
    } else if( site1->get_flippable() ) {
      site1->flip( *site2 , dirs_type );
    }

    // having twiddled and flipped one site, find the best combination of
    // site1_dirs and site2_dirs of the same type and add them to site
    // coords. This is because sometimes there can be more than one
    // dir of the same type that is neither flippable nor twiddlable
    // the first example seen being aniline NH2 for DONORS
    float best_dot = -1.0;
    int best1 = -1 , best2 = -1;
    for( int ii = 0 ; ii < nsd1 ; ++ii ) {
      if( dirs_type != site1->direction_type( ii ) ) {
        continue;
      }
      for( int j = 0 ; j < nsd2; ++j ) {
        if( dirs_type != site2->direction_type( j ) ) {
          continue;
        }
        float this_dot = DACLIB::dot_product( site1_dirs + 3 * ii , site2_dirs + 3 * j );
        if( this_dot > best_dot ) {
          best1 = ii;
          best2 = j;
          best_dot = this_dot;
        }
      }
    }
    if( -1 != best1 && -1 != best2 ) {
      add_site_coords( site1->coords() , site1->direction( best1 ) , cds1 );
      add_site_coords( site2->coords() , site2->direction( best2 ) , cds2 );
    }

    if( made_site1 ) {
      delete site1;
    }
    if( made_site2 ) {
      delete site2;
    }

  }

}


#include <stdio.h>
#include <math.h>
#include <sys/stat.h>

#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <memory>
#include <functional>
#include <algorithm>
#include <map>
#include <random>

#ifdef FILESYSTEM_EXPERIMENTAL
#include <experimental/filesystem>
namespace filesystem = std::experimental::filesystem;
#elif defined(STD_FILESYSTEM)
#include <filesystem>
namespace filesystem = std::filesystem;
#else
#include <boost/filesystem>
#endif

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include "opencv2/opencv_modules.hpp"
#ifdef HAVE_OPENCV_XFEATURES2D
#include <opencv2/xfeatures2d.hpp>
#endif

#include <QApplication>
#include <QCommandLineParser>
#include <QMessageBox>
#include <QtWidgets>
#include <QInputDialog>
#include <QTimer>

#define GLM_ENABLE_EXPERIMENTAL

#include "util.h"
#include "hull.h"
#include "ImageMeta.h"
#include "output.h"
#include "ImageWindow.hh"
#include "Calibration.hh"

#define DEBUG

int main(int argc, char **argv)
//-----------------------------
{
   QApplication a(argc, argv);
   std::stringstream errs;
   QApplication::setApplicationName("Trainer");
   QApplication::setApplicationVersion("0.1");
   QCommandLineParser parser;
   parser.setApplicationDescription("Trainer");
   parser.addHelpOption();
   parser.addVersionOption();
   parser.addOption(QCommandLineOption("t", "Train image", "train-image", ""));
   parser.addOption(QCommandLineOption("q", "Query image", "query-image", ""));
   parser.addOption(QCommandLineOption("c",
                                       "Camera Calibration. Either -c fx,fy,ox,oy (no spaces or surrounded by quotes)\n"
                                       "or -c file where file contains the same comma delimited form\n"
                                       "or a YAML file with a .yaml extension containing for eg\n"
                                       "fx: 123,45\n"
                                       "fy: 456.78 ...", "calibration", ""));
   parser.process(a);
   std::string image_filename = parser.value("t").toStdString();
   std::string query_image_filename = parser.value("q").toStdString();
   std::string calibration_arg = parser.value("c").toStdString();
   std::unique_ptr<Calibration> calibration;
   if (calibration_arg.empty())
      std::cout << "WARNING: No camera calibration specified. Pose option not supported unless calibration read in UI file menu.\n";
   else
   {
      filesystem::path calibration_file(calibration_arg);
      if (filesystem::exists(calibration_file))
      {
         calibration.reset(new Calibration);
         if (! calibration->open(calibration_file.string(), &errs))
         {
            std::cerr << "Calibration file error: " << errs.str() << std::endl;
            calibration.reset(nullptr);
            return 1;
         }
      }
      else
      {
         calibration.reset(new Calibration);
         if (! calibration->set(calibration_arg, &errs))
         {
            std::cerr << "Calibration argument error: " << errs.str() << std::endl;
            return 1;
         }
      }
   }

   ImageWindow imgwin(&a);
   if (calibration)
      imgwin.set_calibration(calibration);
   imgwin.showMaximized();
   if (! image_filename.empty())
   {
      filesystem::path imagefile(image_filename);
      if (filesystem::is_regular_file(imagefile))
      {
         if (!imgwin.load_train_img(image_filename, &errs))
         {
            std::cerr << "WARNING: Command line image file " << image_filename << " not loaded (" << errs.str() << std::endl;
            return 1;
         }
      }
      else
      {
         std::cerr << "WARNING: Command line image file " << image_filename << " not found" << std::endl;
         return 1;
      }
   }
   if (! query_image_filename.empty())
   {
      std::stringstream errs;
      filesystem::path imagefile(query_image_filename);
      if (filesystem::is_regular_file(imagefile))
      {
         if (!imgwin.load_query_img(query_image_filename, &errs))
            std::cerr << "WARNING: Command line image file " << query_image_filename << " not loaded (" << errs.str() << std::endl;
      }
      else
         std::cerr << "WARNING: Command line image file " << query_image_filename << " not found" << std::endl;
   }
   a.exec();
}

cv::Rect adjusted_region(const cv::Rect& region)
//--------------------------------
{
   cv::Rect r(region);
   if (r.width < 0)
   {
      r.x += r.width;
      r.width *= -1;
   }
   if (r.height < 0)
   {
      r.y += r.height;
      r.height *= -1;
   }
   return r;
}

void msg(const char *psz)
//-------------------------------
{
   QMessageBox msgBox;
   msgBox.setText(QString(psz));
   msgBox.exec();
}

void msg(const std::stringstream &errs)
//-------------------------------
{
   QMessageBox msgBox;
   msgBox.setText(QString(errs.str().c_str()));
   msgBox.exec();
}

/*
inline void counterclockwise_sort(std::vector<cv::Point2f>& vertices)
//-------------------------------------------------------------------
{
   cv::Point2f pt1 = vertices[0], pt2 = vertices[1], pt3 = vertices[2];
   cv::Point2f centroid(pt1.x, pt1.y);
   centroid.x += pt2.x; centroid.y += pt2.y;
   centroid.x += pt3.x; centroid.y += pt3.y;
   centroid.x /= 3; centroid.y /= 3;
   const float angle1 = atan2f(pt1.y - centroid.y, pt1.x - centroid.x);
   const float angle2 = atan2f(pt2.y - centroid.y, pt2.x - centroid.x);
   const float angle3 = atan2f(pt3.y - centroid.y, pt3.x - centroid.x);
   if (angle1 > angle2)
   {
      cv::Point2f tmppt = vertices[0];
      vertices[0] = vertices[1];
      vertices[1] = tmppt;
   }
   if (angle1 > angle3)
   {
      cv::Point2f tmppt = vertices[0];
      vertices[0] = vertices[2];
      vertices[2] = tmppt;
   }
   if (angle2 > angle3)
   {
      cv::Point2f tmppt = vertices[1];
      vertices[1] = vertices[2];
      vertices[2] = tmppt;
   }
}

inline bool clockwise_less(cv::Point2f a, cv::Point2f b, cv::Point2f center)
//--------------------------------------------------------------------------
{
   if (a.x - center.x >= 0 && b.x - center.x < 0)
      return true;
   if (a.x - center.x < 0 && b.x - center.x >= 0)
      return false;
   if (a.x - center.x == 0 && b.x - center.x == 0) {
      if (a.y - center.y >= 0 || b.y - center.y >= 0)
         return a.y > b.y;
      return b.y > a.y;
   }

   // compute the cross product of vectors (center -> a) x (center -> b)
   float det = (a.x - center.x) * (b.y - center.y) - (b.x - center.x) * (a.y - center.y);
   if (det < 0)
      return true;
   if (det > 0)
      return false;

   // points a and b are on the same line from the center
   // check which point is closer to the center
   float d1 = (a.x - center.x) * (a.x - center.x) + (a.y - center.y) * (a.y - center.y);
   float d2 = (b.x - center.x) * (b.x - center.x) + (b.y - center.y) * (b.y - center.y);
   return d1 > d2;
}

bool find_normal(Calibration *calibration_values, imeta::ImageMeta& image_meta,
                 const std::vector<cv::KeyPoint>& hull_kpts, const std::vector<cv::KeyPoint>& train_kpts,
                 cv::Mat& normal, cv::Mat& rotated_normal)
//-------------------------------------------------------------------------------------------------------
{
   std::stringstream ss;
   std::vector<cv::KeyPoint> top_keypoints((hull_kpts.size() >= 9) ? hull_kpts : train_kpts);
   cv::KeyPointsFilter::removeDuplicated(top_keypoints);
   cv::KeyPointsFilter::retainBest(top_keypoints, 60);
   std::vector<cv::Point2f> pts;
   cv::KeyPoint::convert(top_keypoints, pts);
   std::vector<cv::Vec6f> triangle_vertices;

   cv::Mat R, RM;
   getRotationMatrixFromMat<double>(R, image_meta.rotation_vector, 4, CV_64FC1);
   //R = cv::Mat::eye(4, 4, CV_64FC1);
   unsigned X, Y;
   switch (image_meta.device_rotation)
   {
      case 90: X = AXIS_Y; Y = AXIS_MINUS_X; break;
      case 180: X = AXIS_MINUS_X; Y = AXIS_MINUS_Y; break;
      case 270: X = AXIS_MINUS_Y; Y = AXIS_X; break;
      default: X = AXIS_X; Y = AXIS_Y; break;
   }
   remapCoordinateSystem<double>(R, X, Y, RM, CV_64FC1);
   cv::Mat RMI = RM.t();
   std::cout << RM << std::endl;
   normal = cv::Mat::zeros(1, 3, CV_64FC1);
   rotated_normal = cv::Mat::zeros(1, 3, CV_64FC1);
   double *an = normal.ptr<double>(0);
   double *ran = rotated_normal.ptr<double>(0);
   int c = 0;
   ss << "clf;" << std::endl;
   std::default_random_engine generator;
//   std::uniform_int_distribution<size_t> distribution(0, n);
   constexpr double min_area = 50;
   std::vector<cv::Point2f> remaining_pts;
   while (pts.size() >= 3)
   {
      std::vector<cv::Point2f> vertices;
      std::vector<size_t> indices;
      size_t n = pts.size();
      for (size_t i=0; i<n; i++)
         indices.push_back(i);
      std::shuffle(indices.begin(), indices.end(), generator);
      auto it = indices.begin();
      int retries = 0;
      while ( it != indices.end())
      {
         vertices.clear();
         for (int i=0; i<3; i++)
         {
            size_t j = *it;
            vertices.push_back(pts[j]);
            if (++it == indices.end())
               break;
         }
         if (vertices.size() == 3)
         {
            //counterclockwise_sort(vertices);
            cv::Point2f& pt1 = vertices[0], &pt2 = vertices[1], &pt3 = vertices[2];
            cv::Point2f centroid(pt1.x, pt1.y);
            centroid.x += pt2.x; centroid.y += pt2.y;
            centroid.x += pt3.x; centroid.y += pt3.y;
            centroid.x /= 3; centroid.y /= 3;
            std::sort(std::begin(vertices), std::end(vertices),
                      [centroid](const cv::Point2f &lhs, const cv::Point2f &rhs) -> bool
                            //----------------------------------------------------------------------------------
                      {
                         return (clockwise_less(lhs, rhs, centroid));
                      });
            cv::Point2d v1 = vertices[0], v2 = vertices[1], v3 = vertices[2];
            double a[] { v1.x, v1.y, 1.0f, v2.x, v2.y, 1.0f, v3.x, v3.y, 1.0f};
            cv::Mat A(3, 3, CV_64FC1, static_cast<void *>(a));
            double area = 0.5 * fabs(cv::determinant(A));
            if (area >= min_area)
            {
               cv::Mat p1 = calibration_values->image2camera(v1, 4).t();
               cv::Mat p2 = calibration_values->image2camera(v2, 4).t();
               cv::Mat p3 = calibration_values->image2camera(v3, 4).t();
               cv::Mat vec1 = p1.rowRange(0, 3) - p2.rowRange(0, 3);
               cv::Mat vec2 = p3.rowRange(0, 3) - p2.rowRange(0, 3);
               cv::Vec3d n = vec1.cross(vec2);
               double mag = sqrt(n.ddot(n));
               n[0] /= mag; n[1] /= mag; n[2] /= mag;
               std::cout << v1 << " " << v2 << " " << v3 << " " << area << " *** " << n << std::endl;
               an[0] += n[0]; an[1] += n[1]; an[2] += n[2];

               p1 = RMI * p1;
               p2 = RMI * p2;
               p3 = RMI * p3;
               vec1 = p1.rowRange(0, 3) - p2.rowRange(0, 3);
               vec2 = p3.rowRange(0, 3) - p2.rowRange(0, 3);
               n = vec1.cross(vec2);
               mag = sqrt(n.ddot(n));
               n[0] /= mag; n[1] /= mag; n[2] /= mag;
               std::cout << v1 << " " << v2 << " " << v3 << " " << area << " *** " << n << std::endl;
               ran[0] += n[0]; ran[1] += n[1]; ran[2] += n[2];

               c++;
            }
            else
            {
               remaining_pts.push_back(v1);
               remaining_pts.push_back(v2);
               remaining_pts.push_back(v3);
               std::cout << v1 << " " << v2 << " " << v3 << " " << area << " reject" << std::endl;
            }
         }
      }
      pts.clear();
      if ( (c < 10) && (remaining_pts.size() > 0) && (retries++ < 3) )
         pts = std::move(remaining_pts);
   }

//   cv::Subdiv2D triangulator(bb);
//   triangulator.insert(pts);
//   triangulator.getTriangleList(triangle_vertices);
//   for (cv::Vec6f triangle : triangle_vertices)
//   {
//   }
   if (c == 0)
      return false;
   double cf = static_cast<double>(c);
   an[0] /= cf; an[1] /= cf; an[2] /= cf;
   ran[0] /= cf; ran[1] /= cf; ran[2] /= cf;
   std::cout << normal << " " << rotated_normal << std::endl;
   return true;
}
*/

//void barycentric_calc(std::vector<cv::KeyPoint>& keypoints, cv::Point2f centroid, bool is_hull_only, float avg_response,
//                      cv::Mat trainImgRGB, std::vector<barycentric_info_t> &barycentrics)
////---------------------------------------------------------------------------------------------------------------------
//{
//   std::vector<cv::KeyPoint> triangle_candidates;
//   for (int i=0; i< static_cast<int>(keypoints.size()); i++)
//   {
//      KeyPointExtra_t kpe;
//      cv::KeyPoint& kp = keypoints[i];
//      if (kp.response < avg_response)
//         continue;
//      kpe.class_id = static_cast<uint32_t>(kp.class_id);
//      if ( (is_hull_only) && (! kpe.is_hull) )
//         continue;
//      triangle_candidates.push_back(kp);
//   }
//   std::sort(triangle_candidates.begin(), triangle_candidates.end(),
//             [](const cv::KeyPoint &kp1, const cv::KeyPoint &kp2) -> bool
//                   //----------------------------------------------------------
//             {
//                return kp2.response < kp1.response;
//             });
//   std::vector<std::vector<cv::KeyPoint>> triangles;
//   std::unique_ptr<Triangulator> triangulator(new OrderedTriangulator());
//   triangulator->triangulate(triangle_candidates, triangles);
//#ifdef DEBUG
//   cv::Mat debug_img;
//   trainImgRGB.copyTo(debug_img);
//   int tn = 0;
//#endif
//   for (std::vector<cv::KeyPoint> triangle : triangles)
//   {
//#ifdef DEBUG
//      draw_triangle(debug_img, triangle);
//      cv::Mat one_triangleim; trainImgRGB.copyTo(one_triangleim);
//      draw_triangle(one_triangleim, triangle);
//      std::stringstream ss;
//      ss << "triangle-" << std::setfill('0') << std::setw(3) << tn++ << ".png";
//      cv::imwrite(ss.str(), one_triangleim);
//#endif
//      barycentric_info info;
//      std::vector<float> coordinates;
//      if (barycentric(triangle, centroid, coordinates))
//      {
//         float data[]{triangle[0].pt.x, triangle[0].pt.y, 1.0f,
//                      triangle[1].pt.x, triangle[1].pt.y, 1.0f,
//                      triangle[2].pt.x, triangle[2].pt.y, 1.0f};
//         cv::Mat A(3, 3, CV_32FC1, static_cast<void *>(data));
//         info.area = 0.5f * fabs(static_cast<float>(cv::determinant(A)));
//         for (int i=0; i<3; i++)
//         {
//            cv::KeyPoint &kp = triangle[i];
//            KeyPointExtra_t kpe;
//            kpe.class_id = (uint32_t) kp.class_id;
//            info.vertex_keypointno[i] = kpe.no;
//            info.coordinates[i] = coordinates[i];
//         }
//         barycentrics.push_back(info);
//      }
//   }
//
////      cv::imshow("win", debug_img); cv::waitKey(0);
//   cv::imwrite("triangles.png", debug_img);
//}

//int combinations(const std::vector<cv::KeyPoint>& keypoints, int r,
//                 std::function<bool (std::vector<cv::KeyPoint>)> fn_next_combination);
//void xratio(const std::vector<cv::KeyPoint>& keypoints, const cv::Point2f& centroid,
//            std::vector<xratio_info_t>& xratios);
//
//int combinations(const std::vector<cv::KeyPoint>& v, int r,
//                 std::function<bool (const std::vector<cv::KeyPoint>)> fn_next_combination)
////------------------------------------------------------------------------------------------------------------------------------
//{
//   int n = static_cast<int>(v.size());
//   std::vector<bool> b(n);
//   std::fill(b.begin(), b.begin() + r, true);
//   do
//   {
//      std::vector<cv::KeyPoint> combination;
//      for (int i = 0; i < n; ++i)
//      {
//         if (b[i])
//            combination.push_back(v[i]);
//      }
//      if (! fn_next_combination(combination))
//         break;
//   } while (std::prev_permutation(b.begin(), b.end()));
//   return 0;
//}
//
//void xratio(const std::vector<cv::KeyPoint>& keypoints, const cv::Point2f& centroid,
//            std::vector<xratio_info_t>& xratios)
////----------------------------------------------------------------------------------
//{
//   std::function<bool (std::vector<cv::KeyPoint>)> fn =
//         [&centroid, &xratios](std::vector<cv::KeyPoint> vertices) -> bool
//               //------------------------------------------------------------------------
//         {
//            int  i = 0;
//            float data[] {vertices[0].pt.x, vertices[0].pt.y, 1.0f,
//                          vertices[1].pt.x, vertices[1].pt.y, 1.0f,
//                          vertices[2].pt.x, vertices[2].pt.y, 1.0f};
//            cv::Mat A(3, 3, CV_32FC1, static_cast<void *>(data));
//            float area = 0.5f*fabs(static_cast<float>(cv::determinant(A)));
//            if (area > 0.00001)
//               return true;
//            data[6] = centroid.x;
//            data[7] = centroid.y;
//            A = cv::Mat(3, 3, CV_32FC1, static_cast<void *>(data));
//            area = 0.5f*fabs(static_cast<float>(cv::determinant(A)));
//            if (area > 0.00001)
//               return true;
//            KeyPointExtra_t kpe;
//            kpe.no = 0;
//            kpe.is_hull = kpe.is_selected = 0;
//            kpe.imgIdx = 255;
//            cv::KeyPoint kpc(centroid, 0, -1, -1);
//            kpc.class_id = kpe.class_id;
//            vertices.push_back(kpc);
//            std::sort(vertices.begin(), vertices.end(),
//                      [](const cv::KeyPoint &kp1, const cv::KeyPoint &kp2) -> bool
//                            //--------------------------------------------------------
//                      {
//                         float dx = kp1.pt.x, dy = kp1.pt.y;
//                         float d1 = dx*dx + dy*dy;
//                         dx = kp2.pt.x; dy = kp2.pt.y;
//                         return d1 < (dx*dx + dy*dy);
//                      });
//            cv::Point2f &p1 = vertices[0].pt, &p2 = vertices[1].pt,
//                  &p3 = vertices[2].pt, &p4 = vertices[3].pt;
//
//            xratio_info info;
//            info.xratio = (euclidean2(p1, p3) * euclidean2(p2, p4)) / euclidean2(p1, p4) * euclidean2(p2, p3);
//            i = 0;
//            for (cv::KeyPoint kp : vertices)
//            {
//               kpe.class_id = static_cast<uint32_t>(kp.class_id);
//               if (kpe.imgIdx == 255)
//                  info.centre_index = i;
//               info.vertex_keypointno[i++] = kpe.no;
//            }
//         };
//   combinations(keypoints, 3, fn);
//}


// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.

#pragma once

#include <dlib/matrix.h>
#include "ocos.h"

struct KernelInverse : BaseKernel {
  KernelInverse(OrtApi api) : BaseKernel(api) {
  }

  void Compute(OrtKernelContext* context) {
    // Setup inputs
    const OrtValue* input_X = ort_.KernelContext_GetInput(context, 0);
    const float* X = ort_.GetTensorData<float>(input_X);

    // Setup output
    OrtTensorDimensions dimensions(ort_, input_X);
    if (dimensions.size() != 2) {
      throw std::runtime_error("Only 2-d matrix supported.");
    }

    OrtValue* output0 = ort_.KernelContext_GetOutput(context, 0, dimensions.data(), dimensions.size());
    float* out0 = ort_.GetTensorMutableData<float>(output0);

    OrtTensorTypeAndShapeInfo* output_info = ort_.GetTensorTypeAndShape(output0);
    int64_t size = ort_.GetTensorShapeElementCount(output_info);
    ort_.ReleaseTensorTypeAndShapeInfo(output_info);

    dlib::matrix<float> dm(dimensions[0], dimensions[1]);
    // Do computation
    for (int64_t i = 0; i < size; i++) {
      out0[i] = dm(i / dimensions[1], i % dimensions[1]);
    }
  }
};

struct CustomOpInverse : Ort::CustomOpBase<CustomOpInverse, KernelInverse> {
  void* CreateKernel(OrtApi api, const OrtKernelInfo* info) const {
    return new KernelInverse(api);
  }

  const char* GetName() const {
    return "Inverse";
  }

  size_t GetInputTypeCount() const {
    return 1;
  }

  ONNXTensorElementDataType GetInputType(size_t index) const {
    return ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT;
  }

  size_t GetOutputTypeCount() const {
    return 1;
  }

  ONNXTensorElementDataType GetOutputType(size_t index) const {
    return ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT;
  }
};


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

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

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

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <boost/date_time.hpp>
#include <boost/make_shared.hpp>
#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/coupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>
#include <ql/event.hpp>
#include <ql/quotes/compositequote.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/yield/zerospreadedtermstructure.hpp>
#include <qle/pricingengines/discountingforwardbondengine.hpp>

namespace QuantExt {

DiscountingForwardBondEngine::DiscountingForwardBondEngine(
    const Handle<YieldTermStructure>& discountCurve, const Handle<YieldTermStructure>& incomeCurve,
    const Handle<YieldTermStructure>& bondReferenceYieldCurve, const Handle<Quote>& bondSpread,
    const Handle<DefaultProbabilityTermStructure>& bondDefaultCurve, const Handle<Quote>& bondRecoveryRate,
    Period timestepPeriod, boost::optional<bool> includeSettlementDateFlows, const Date& settlementDate,
    const Date& npvDate)
    : discountCurve_(discountCurve), incomeCurve_(incomeCurve), bondReferenceYieldCurve_(bondReferenceYieldCurve),
      bondSpread_(bondSpread), bondDefaultCurve_(bondDefaultCurve), bondRecoveryRate_(bondRecoveryRate),
      timestepPeriod_(timestepPeriod), includeSettlementDateFlows_(includeSettlementDateFlows),
      settlementDate_(settlementDate), npvDate_(npvDate) {

    bondReferenceYieldCurve_ =
        bondSpread_.empty() ? bondReferenceYieldCurve
                            : Handle<YieldTermStructure>(
                                  boost::make_shared<ZeroSpreadedTermStructure>(bondReferenceYieldCurve, bondSpread_));
    registerWith(discountCurve_);           // curve for discounting of the forward derivative contract. OIS, usually.
    registerWith(incomeCurve_);             // this is a curve for compounding of the bond
    registerWith(bondReferenceYieldCurve_); // this is the bond reference curve, for discounting, usually RePo
    registerWith(bondSpread_);
    registerWith(bondDefaultCurve_);
    registerWith(bondRecoveryRate_);
}

void DiscountingForwardBondEngine::calculate() const {
    // Do some checks on data
    QL_REQUIRE(!discountCurve_.empty(), "discounting term structure handle is empty");
    QL_REQUIRE(!incomeCurve_.empty(), "income term structure handle is empty");
    QL_REQUIRE(!bondReferenceYieldCurve_.empty(), "bond rerference term structure handle is empty");

    Date npvDate = npvDate_; // this is today when the valuation occurs
    if (npvDate == Null<Date>()) {
        npvDate = (*discountCurve_)->referenceDate();
    }
    Date settlementDate = settlementDate_; //
    if (settlementDate == Null<Date>()) {
        settlementDate = (*discountCurve_)->referenceDate();
    }

    Date maturityDate =
        arguments_.fwdMaturityDate; // this is the date when the forward is executed, i.e. cash and bond change hands

    Real cmpPayment = arguments_.compensationPayment;
    if (cmpPayment == Null<Real>()) {
        cmpPayment = 0.0;
    }
    Date cmpPaymentDate = arguments_.compensationPaymentDate;
    if (cmpPaymentDate == Null<Date>()) {
        cmpPaymentDate = npvDate;
    }

    // in case that the premium payment has occured in the past, we set the amount to 0. the date itself is set to the
    // npvDate to have a valid date for "discounting"
    Date cmpPaymentDate_use = cmpPaymentDate >= npvDate ? cmpPaymentDate : maturityDate;
    cmpPayment = cmpPaymentDate >= npvDate ? cmpPayment : 0.0; // premium cashflow is not relevant for npv if in the
                                                               // past

    // initialize
    results_.value = 0.0;               // this is today's npv of the forward contract
    results_.underlyingSpotValue = 0.0; // this is today's value of the "restricted bond". Restricted means that only
                                        // cashflows after maturity are taken into account.
    results_.forwardValue = 0.0;        // this the value of the forward contract just before maturity

    bool dirty = arguments_.settlementDirty;

    results_.underlyingSpotValue = calculateBondNpv(npvDate, maturityDate); // cashflows before maturity will be ignored

    boost::tie(results_.forwardValue, results_.value) = calculateForwardContractPresentValue(
        results_.underlyingSpotValue, cmpPayment, npvDate, maturityDate, cmpPaymentDate_use, dirty);
}

Real DiscountingForwardBondEngine::calculateBondNpv(Date npvDate, Date computeDate) const {
    Real npvValue = 0.0;
    Size numCoupons = 0;
    bool hasLiveCashFlow = false;

    // handle case where we wish to price simply with benchmark curve and scalar security spread
    // i.e. credit curve term structure (and recovery) have not been specified
    // we set the default probability and recovery rate to zero in this instance (issuer credit worthiness already
    // captured within security spread)
    boost::shared_ptr<DefaultProbabilityTermStructure> creditCurvePtr =
        bondDefaultCurve_.empty()
            ? boost::make_shared<QuantLib::FlatHazardRate>(npvDate, 0.0, bondReferenceYieldCurve_->dayCounter())
            : bondDefaultCurve_.currentLink();
    Rate recoveryVal = bondRecoveryRate_.empty() ? 0.0 : bondRecoveryRate_->value(); // setup default bond recovery rate

    // load the shared pointer into bd
    boost::shared_ptr<Bond> bd = arguments_.underlying;
    for (Size i = 0; i < bd->cashflows().size(); i++) {
        // Recovery amount is computed over the whole time interval (npvDate,maturityOfBond)

        if (bd->cashflows()[i]->hasOccurred(
                computeDate, includeSettlementDateFlows_)) // Cashflows before computeDate not relevant for npv
            continue;

        /* The amount recovered in the case of default is the recoveryrate*Notional*Probability of
           Default; this is added to the NPV value. For coupon bonds the coupon periods are taken
           as the timesteps for integrating over the probability of default.
        */
        boost::shared_ptr<Coupon> coupon = boost::dynamic_pointer_cast<Coupon>(bd->cashflows()[i]);

        if (coupon) {
            numCoupons++;
            Date startDate = coupon->accrualStartDate();
            Date endDate = coupon->accrualEndDate();
            Date effectiveStartDate = (startDate <= computeDate && computeDate <= endDate) ? computeDate : startDate;
            Date defaultDate = effectiveStartDate + (endDate - effectiveStartDate) / 2;
            Probability P = creditCurvePtr->defaultProbability(effectiveStartDate, endDate);

            npvValue += coupon->nominal() * recoveryVal * P * bondReferenceYieldCurve_->discount(defaultDate);
        }

        hasLiveCashFlow = true; // check if a cashflow  is available after the date of valuation.

        // Coupon value is discounted future payment times the survival probability
        Probability S = creditCurvePtr->survivalProbability(bd->cashflows()[i]->date()) /
                        creditCurvePtr->survivalProbability(computeDate);
        npvValue += bd->cashflows()[i]->amount() * S * bondReferenceYieldCurve_->discount(bd->cashflows()[i]->date());
    }

    // the ql instrument might not yet be expired and still have not anything to value if
    // the computeDate > evaluation date
    if (!hasLiveCashFlow)
        return 0.0;

    if (bd->cashflows().size() > 1 && numCoupons == 0) {
        QL_FAIL("DiscountingForwardBondEngine does not support bonds with multiple cashflows but no coupons");
    }

    boost::shared_ptr<Coupon> firstCoupon = boost::dynamic_pointer_cast<Coupon>(bd->cashflows()[0]);
    if (firstCoupon) {
        Date startDate = computeDate; // face value recovery starting with computeDate
        while (startDate < bd->cashflows()[0]->date()) {
            Date stepDate = startDate + timestepPeriod_;
            Date endDate = (stepDate > bd->cashflows()[0]->date()) ? bd->cashflows()[0]->date() : stepDate;
            Date defaultDate = startDate + (endDate - startDate) / 2;
            Probability P = creditCurvePtr->defaultProbability(startDate, endDate);

            npvValue += firstCoupon->nominal() * recoveryVal * P * bondReferenceYieldCurve_->discount(defaultDate);
            startDate = stepDate;
        }
    }
    /* If there are no coupon, as in a Zero Bond, we must integrate over the entire period from npv date to
       maturity. The timestepPeriod specified is used as provide the steps for the integration. This only applies
       to bonds with 1 cashflow, identified as a final redemption payment.
    */
    if (bd->cashflows().size() == 1) {
        boost::shared_ptr<Redemption> redemption = boost::dynamic_pointer_cast<Redemption>(bd->cashflows()[0]);
        if (redemption) {
            Date startDate = computeDate;
            while (startDate < redemption->date()) {
                Date stepDate = startDate + timestepPeriod_;
                Date endDate = (stepDate > redemption->date()) ? redemption->date() : stepDate;
                Date defaultDate = startDate + (endDate - startDate) / 2;
                Probability P = creditCurvePtr->defaultProbability(startDate, endDate);

                npvValue += redemption->amount() * recoveryVal * P * bondReferenceYieldCurve_->discount(defaultDate);
                startDate = stepDate;
            }
        }
    }
    return npvValue * arguments_.bondNotional;
}

boost::tuple<Real, Real> DiscountingForwardBondEngine::calculateForwardContractPresentValue(
    Real spotValue, Real cmpPayment, Date npvDate, Date computeDate, Date cmpPaymentDate, bool dirty) const {
    // here we go with the true forward computation
    Real forwardBondValue = 0.0;
    Real forwardContractPresentValue = 0.0;
    Real forwardContractForwardValue = 0.0;

    // handle case where we wish to price simply with benchmark curve and scalar security spread
    // i.e. credit curve term structure (and recovery) have not been specified
    // we set the default probability and recovery rate to zero in this instance (issuer credit worthiness already
    // captured within security spread)
    boost::shared_ptr<DefaultProbabilityTermStructure> creditCurvePtr =
        bondDefaultCurve_.empty()
            ? boost::make_shared<QuantLib::FlatHazardRate>(npvDate, 0.0, bondReferenceYieldCurve_->dayCounter())
            : bondDefaultCurve_.currentLink();
    Rate recoveryVal = bondRecoveryRate_.empty() ? 0.0 : bondRecoveryRate_->value(); // setup default bond recovery rate

    // load the shared pointer into bd
    boost::shared_ptr<Bond> bd = arguments_.underlying;

    // the case of dirty strike corresponds here to an accrual of 0.0. This will be convenient in the code.
    Real accruedAmount =
        dirty ? 0.0 : bd->accruedAmount(computeDate) * bd->notional(computeDate) / 100 * arguments_.bondNotional;

    /* Discounting and compounding, taking account of possible bond default before delivery*/

    forwardBondValue =
        spotValue / (incomeCurve_->discount(computeDate)); // compounding to date of maturity of forward contract

    // Subtract strike at maturity. Regarding accrual (i.e. strike is given clean vs dirty) there are two
    // cases: long or short.

    // Long: forwardBondValue - strike_dirt = (forwardBondValue - accrual) - strike_clean
    // Short: strike_dirt - forwardBondValue = strike_clean - (forwardBondValue - accrual)
    // In total:
    forwardContractForwardValue = (*arguments_.payoff)(forwardBondValue - accruedAmount);

    // forwardContractPresentValue adjusted for potential default before computeDate:
    forwardContractPresentValue =
        forwardContractForwardValue * (discountCurve_->discount(computeDate)) *
            creditCurvePtr->survivalProbability(computeDate) -
        cmpPayment *
            (discountCurve_->discount(cmpPaymentDate)); // The forward is a derivative. We use "OIS curve" to discount.
                                                        // We subtract the potential payment due to Premium.

    // Take account of face value recovery:
    // A) Recovery for time period when coupons are present
    for (Size i = 0; i < bd->cashflows().size(); i++) {
        if (bd->cashflows()[i]->hasOccurred(
                npvDate, includeSettlementDateFlows_)) // Cashflows before npvDate not relevant for npv
            continue;
        if (bd->cashflows()[i]->date() >=
            computeDate) // Cashflows after computeDate do not fall into the forward period
            continue;
        boost::shared_ptr<Coupon> coupon = boost::dynamic_pointer_cast<Coupon>(bd->cashflows()[i]);

        if (coupon) {
            Date startDate = coupon->accrualStartDate();
            Date endDate = coupon->accrualEndDate();
            Date effectiveStartDate = (startDate <= npvDate && npvDate <= endDate) ? npvDate : startDate;
            Date effectiveEndDate = (startDate <= computeDate && computeDate <= endDate) ? computeDate : endDate;
            Date defaultDate = effectiveStartDate + (effectiveEndDate - effectiveStartDate) / 2;
            Probability P = creditCurvePtr->defaultProbability(effectiveStartDate, effectiveEndDate);

            forwardContractPresentValue +=
                (*arguments_.payoff)(coupon->nominal() * arguments_.bondNotional * recoveryVal - accruedAmount) * P *
                (discountCurve_->discount(defaultDate));
        }
    }

    // B) Recovery for time period before coupons are present
    boost::shared_ptr<Coupon> firstCoupon = boost::dynamic_pointer_cast<Coupon>(bd->cashflows()[0]);
    if (firstCoupon) {
        Date startDate = npvDate; // face value recovery starting with npvDate
        Date stopDate = std::min(bd->cashflows()[0]->date(), computeDate);
        while (startDate < stopDate) {
            Date stepDate = startDate + timestepPeriod_;
            Date endDate = (stepDate > stopDate) ? stopDate : stepDate;
            Date defaultDate = startDate + (endDate - startDate) / 2;
            Probability P = creditCurvePtr->defaultProbability(startDate, endDate);

            forwardContractPresentValue +=
                (*arguments_.payoff)(firstCoupon->nominal() * arguments_.bondNotional * recoveryVal - accruedAmount) *
                P * (discountCurve_->discount(defaultDate));
            startDate = stepDate;
        }
    }
    // C) ZCB
    /* If there are no coupon, as in a Zero Bond, we must integrate over the entire period from npv date to
       maturity. The timestepPeriod specified is used as provide the steps for the integration. This only applies
       to bonds with 1 cashflow, identified as a final redemption payment.
    */
    if (bd->cashflows().size() == 1) {
        boost::shared_ptr<Redemption> redemption = boost::dynamic_pointer_cast<Redemption>(bd->cashflows()[0]);
        if (redemption) {
            Date startDate = npvDate;
            while (startDate < redemption->date()) {
                Date stepDate = startDate + timestepPeriod_;
                Date endDate = (stepDate > redemption->date()) ? redemption->date() : stepDate;
                Date defaultDate = startDate + (endDate - startDate) / 2;
                Probability P = creditCurvePtr->defaultProbability(startDate, endDate);

                forwardContractPresentValue +=
                    (*arguments_.payoff)(redemption->amount() * arguments_.bondNotional * recoveryVal - accruedAmount) *
                    P * (discountCurve_->discount(defaultDate));
                startDate = stepDate;
            }
        }
    }

    return boost::make_tuple(forwardContractForwardValue, forwardContractPresentValue);
}

} // namespace QuantExt


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE LoopTests
#include "poputil/Loop.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/ElementWise.hpp"
#include "popops/codelets.hpp"
#include "poputil/TileMapping.hpp"
#include "poputil/exceptions.hpp"
#include <boost/test/unit_test.hpp>
#include <poplibs_support/TestDevice.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poplibs_test::util;
using namespace popops;
using namespace poplibs_support;

static std::size_t loopTest(std::size_t begin, std::size_t end,
                            std::size_t step) {
  auto device = createTestDevice(TEST_TARGET, 1, 4);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  auto acc = graph.addVariable(UNSIGNED_INT, {1}, "acc");
  poputil::mapTensorLinearly(graph, acc);

  Sequence uploadProg, downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  auto rawHostAcc = allocateHostMemoryForTensor(acc, "acc", graph, uploadProg,
                                                downloadProg, tmap);

  auto loop = Sequence();
  loop.add(poputil::countedLoop(
      graph, begin, end, step, "/countedLoop", [&](Tensor idx) {
        Sequence prog;
        popops::addInPlace(graph, acc, idx, prog, "/accumulate");
        return prog;
      }));

  Engine engine(graph, Sequence(uploadProg, loop, downloadProg));
  device.bind([&](const Device &d) {
    engine.load(d);
    attachStreams(engine, tmap);
    engine.run(0);
  });

  return *reinterpret_cast<unsigned *>(rawHostAcc.get());
}

BOOST_AUTO_TEST_CASE(LoopTests) {
  BOOST_CHECK_EQUAL(loopTest(1, 2, 1), 1);
  BOOST_CHECK_EQUAL(loopTest(1, 2, 100), 1);
  BOOST_CHECK_EQUAL(loopTest(1, 10, 1), 45); // 1 + 2 + ... + 9
  BOOST_CHECK_EQUAL(loopTest(1, 10, 2), 25); // 1 + 3 + 5 + 7 + 9

  BOOST_CHECK_THROW(loopTest(0, 0, 0), poputil::poplibs_error);
  BOOST_CHECK_THROW(loopTest(0, 1, 0), poputil::poplibs_error);
  BOOST_CHECK_THROW(loopTest(10, 10, 10), poputil::poplibs_error);
}


/**
 * $Id$
 *
 * Software License Agreement (GNU General Public License)
 *
 *  Copyright (C) 2015:
 *
 *    Johann Prankl, prankl@acin.tuwien.ac.at
 *    Aitor Aldoma, aldoma@acin.tuwien.ac.at
 *
 *      Automation and Control Institute
 *      Vienna University of Technology
 *      Gusshausstraße 25-29
 *      1170 Vienn, Austria
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * @author Johann Prankl
 *
 */

#ifndef KP_TSF_MAPPING_HH
#define KP_TSF_MAPPING_HH

#include <iostream>
#include <fstream>
#include <float.h>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <boost/thread/mutex.hpp>
#include <boost/thread.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <boost/shared_ptr.hpp>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include <v4r/features/FeatureDetector.h>
#include <v4r/reconstruction/RefineProjectedPointLocationLK.h>
#include <v4r/keypoints/RigidTransformationRANSAC.h>
#include <v4r/recognition/RansacSolvePnPdepth.h>
#include <v4r/camera_tracking_and_mapping/TSFData.h>
#include <v4r/camera_tracking_and_mapping/TSFFrame.hh>
#include <v4r/camera_tracking_and_mapping/TSFOptimizeBundle.hh>
#include <v4r/core/macros.h>

namespace v4r
{

/**
 * TSFMapping
 */
class V4R_EXPORTS TSFMapping
{
public:

  /**
   * Parameter
   */
  class Parameter
  {
  public:
    cv::Size win_size;
    int max_level;
    cv::TermCriteria termcrit;
    float max_error;                     //100
    int max_count;
    double max_dev_vr_normal;
    double depth_error_scale;
    double max_delta_angle_loop;
    double max_cam_dist_loop;
    double max_delta_angle_eq_pose;
    double max_cam_dist_eq_pose;
    double nnr;
    bool refine_plk;
    bool detect_loops;
    int nb_tracked_frames;
    RefineProjectedPointLocationLK::Parameter plk_param;
    v4r::RansacSolvePnPdepth::Parameter pnp;
    TSFOptimizeBundle::Parameter ba;
    Parameter()
     : win_size(cv::Size(21,21)), max_level(2), termcrit(cv::TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)), max_error(100),
       max_count(500), max_dev_vr_normal(75),
       max_delta_angle_loop(30), max_cam_dist_loop(1.5), max_delta_angle_eq_pose(5), max_cam_dist_eq_pose(0.1),
       nnr(0.95), refine_plk(false), detect_loops(true), nb_tracked_frames(2),
       plk_param(RefineProjectedPointLocationLK::Parameter(5., 0.01, 0.1, 10, 15., 0.3, true, cv::Size(21,21))),
       pnp(v4r::RansacSolvePnPdepth::Parameter(1.5, 0.01, 2000, INT_MIN, 4, 0.015))
    {}
  };

private:
  Parameter param;

  double cos_rad_max_dev_vr_normal;

  cv::Mat_<double> intrinsic;
  Eigen::Matrix3f C;

  bool run, have_thread;

  boost::thread th_obectmanagement;
  boost::thread th_init;

  TSFData *data;

  std::vector<TSFFrame::Ptr> map_frames;

  cv::Mat_<cv::Vec3b> image0, image;
  cv::Mat_<unsigned char> im_gray0, im_gray1, im_gray2;
  cv::Mat_<unsigned char> im_warped;

  std::vector<cv::Point2f> cv_points, cv_points1;
  std::vector<int> converged;
  std::vector<std::vector< cv::DMatch > > matches;

  v4r::FeatureDetector::Ptr detector;
  v4r::FeatureDetector::Ptr descEstimator;
  RefineProjectedPointLocationLK plk;
  v4r::RansacSolvePnPdepth::Ptr pnp;
  cv::FlannBasedMatcher matcher;

  TSFOptimizeBundle ba;

  void operate();

  void initKeypoints(const cv::Mat_<unsigned char> &im, TSFFrame &frame0);
  void addFeatureLinks(TSFFrame &frame0, TSFFrame &frame1, const cv::Mat_<unsigned char> &im0, const cv::Mat_<unsigned char> &im1, const Eigen::Matrix4f &pose0, const Eigen::Matrix4f &pose1, bool is_loop);
  void filterValidPoints3D(std::vector<cv::Point2f> &points, std::vector<Eigen::Vector3f> &points3d, std::vector<Eigen::Vector3f> &normals);
  void getPoints3D(const v4r::DataMatrix2D<Surfel> &cloud, const std::vector<cv::Point2f> &points, std::vector<Eigen::Vector3f> &points3d, std::vector<Eigen::Vector3f> &normals);
  void getKeys3D(const v4r::DataMatrix2D<Surfel> &cloud, const std::vector<cv::KeyPoint> &keys, std::vector<Eigen::Vector3f> &keys3d);
  void filterValidKeys3D(std::vector<cv::KeyPoint> &keys, std::vector<Eigen::Vector3f> &keys3d);
  int addProjectionsPLK(int frame_idx, const v4r::DataMatrix2D<Surfel> &sf_cloud, const std::vector<cv::Point2f> &cv_points, const std::vector<int> &converged, std::vector< std::vector< v4r::triple<int, cv::Point2f, Eigen::Vector3f > > > &projs);
  void addLoops();
  bool refineLK(const TSFFrame &frame0, const TSFFrame &frame1, const cv::Mat_<unsigned char> &im0, const cv::Mat_<unsigned char> &im1, Eigen::Matrix4f &pose01, std::vector<cv::Point2f> &refined1, std::vector<int> &converged1);
  bool ransacPose(const std::vector<Eigen::Vector3f> &query, const std::vector<cv::KeyPoint> &query_keys, const std::vector<Eigen::Vector3f> &train, const std::vector<std::vector< cv::DMatch > > &matches, Eigen::Matrix4f &pose, int &nb_inls);
  void warpImage(const TSFFrame &frame1, const cv::Mat_<unsigned char> &im0, const Eigen::Matrix4f &pose, cv::Mat_<unsigned char> &im_warped, Eigen::Matrix3f &H);

  inline void mapPoint(const cv::Point2f &pt_in, const Eigen::Matrix3f &H, cv::Point2f &pt_out);

public:
  cv::Mat dbg;

  TSFMapping(const Parameter &p=Parameter());
  ~TSFMapping();

  void start();
  void stop();

  inline bool isStarted() {return have_thread;}

  inline void lock(){ data->lock(); }        // threaded object management, so we need to lock
  inline void unlock() { data->unlock(); }

  void reset();
  void setData(TSFData *_data) { data = _data; }

  void setCameraParameter(const cv::Mat &_intrinsic);
  void setParameter(const Parameter &p);
  void setDetectors(const FeatureDetector::Ptr &_detector, const FeatureDetector::Ptr &_descEstimator);

  void optimizeMap();
  void getCameraParameter(cv::Mat &_intrinsic, cv::Mat &_dist_coeffs) { ba.getCameraParameter(_intrinsic,_dist_coeffs); }

  void transformMap(const Eigen::Matrix4f &transform);

  inline const std::vector< std::vector<Eigen::Vector3d> > &getOptiPoints() const {return ba.getOptiPoints();}
  inline const std::vector<TSFFrame::Ptr> &getMap() const { return map_frames; }

  typedef boost::shared_ptr< ::v4r::TSFMapping> Ptr;
  typedef boost::shared_ptr< ::v4r::TSFMapping const> ConstPtr;
};

/*************************** INLINE METHODES **************************/

inline void TSFMapping::mapPoint(const cv::Point2f &pt_in, const Eigen::Matrix3f &H, cv::Point2f &pt_out)
{
  pt_out.x = H(0,0)*pt_in.x + H(0,1)*pt_in.y + H(0,2);
  pt_out.y = H(1,0)*pt_in.x + H(1,1)*pt_in.y + H(1,2);
  float t = H(2,0)*pt_in.x + H(2,1)*pt_in.y + H(2,2);
  pt_out.x /= t;
  pt_out.y /= t;
}

} //--END--

#endif


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

  @copyright 2017 NumScale SAS

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

namespace boost { namespace simd
{
  /*!
    @ingroup group-swar
    @ingroup swar-subadd

    Alternatively  subtract and add packed  floating-point elements from a pack.
    Scalar version is just minus.

    @par Header <boost/simd/function/subadd.hpp>
    @par Example:

    @snippet subadd.cpp subadd

    @par Possible output:

    @snippet subadd.txt subadd

  **/
} }

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

#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/nsm.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    namespace tt = nsm::type_traits;

    struct valmin_ : boost::dispatch::constant_value_<valmin_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,valmin_,boost::dispatch::constant_value_<valmin_>);

      struct value_map
      {
        template<typename X>
        static tt::integral_constant<X,-128> value(boost::dispatch::int8_<X> const&);

        template<typename X>
        static tt::integral_constant<X,-32768> value(boost::dispatch::int16_<X> const&);

        template<typename X>
        static tt::integral_constant<X,-2147483647-1> value(boost::dispatch::int32_<X> const&);

        template<typename X>
        static tt::integral_constant<X,-9223372036854775807LL -1LL> value(boost::dispatch::int64_<X> const&);

        template<typename X>
        static tt::integral_constant<X,X(0)> value(boost::dispatch::uint_<X> const&);

        template<typename X>
        static nsm::single_<0xFF7FFFFF> value(boost::dispatch::single_<X> const&);

        template<typename X>
        static nsm::double_<0xFFEFFFFFFFFFFFFFULL> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, valmin_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::valmin_,valmin);
  }

  template<typename T> BOOST_FORCEINLINE auto Valmin()
  -> decltype(detail::valmin(boost::dispatch::as_<T>{}))
  {
    return detail::valmin( boost::dispatch::as_<T>{} );
  }

  template<typename T> BOOST_FORCEINLINE
  auto Valmin(boost::dispatch::as_<T> const&) BOOST_NOEXCEPT_DECLTYPE(Valmin<T>())
  {
    return Valmin<T>();
  }
} }

#endif