// SPDX-License-Identifier: MIT
#include <fstream>
#include <iostream>
#include <vector>
#include <string>

#include <assimp/cimport.h>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>
#include <assimp/scene.h>
#include <boost/program_options.hpp>
#include <glm/vec2.hpp> // glm::vec2
#include <glm/vec3.hpp> // glm::vec3
#include <glm/vec4.hpp> // glm::vec4

namespace po = boost::program_options;

struct Mesh
{
  glm::vec3 color;

  uint32_t vertex_base;
  uint32_t vertex_count;
  uint32_t index_base;
  uint32_t index_count;
};

struct Vertex
{
  glm::vec3 position;
  glm::vec3 normal;
  glm::vec2 texture_coord;
};

void write_to_file(std::ofstream &output_file, const uint32_t &ui)
{
  output_file.write((char*)&ui, sizeof(uint32_t));
}

void write_to_file(std::ofstream &output_file, const glm::vec3 &vector)
{
  output_file.write((char*)&vector.x, sizeof(glm::vec3::value_type));
  output_file.write((char*)&vector.y, sizeof(glm::vec3::value_type));
  output_file.write((char*)&vector.z, sizeof(glm::vec3::value_type));
}

void write_to_file(std::ofstream &output_file, const glm::vec2 &vector)
{
  output_file.write((char*)&vector.x, sizeof(glm::vec2::value_type));
  output_file.write((char*)&vector.y, sizeof(glm::vec2::value_type));
}

int main(int argc, char *argv[])
{
  std::string file_path;
  std::ofstream output_file;

  // Get arguments.
  {
    bool incomplete_arguments{false};

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "show this help message")
        ("source", po::value<std::string>(), "file to be imported")
        ("out", po::value<std::string>(), "exported file name")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help"))
    {
      std::cout << desc << "\n";
      return 0;
    }

    if (vm.count("source"))
    {
      file_path = vm["source"].as<std::string>();
    }
    else
    {
      incomplete_arguments = true;
      std::cout << "Source was not set.\n";
    }

    if (vm.count("out"))
    {
      output_file.open(vm["out"].as<std::string>());
    }
    else
    {
      incomplete_arguments = true;
      std::cout << "Output was not set.\n";
    }

    if(incomplete_arguments) return 0;
  }

  // Export file.
  {
    Assimp::Importer importer{};
    const aiScene* p_scene{nullptr};

    std::vector<Mesh> meshes{};
    std::vector<Vertex> vertexes{};
    std::vector<uint32_t> indexes{};

    p_scene = importer.ReadFile(file_path.c_str(),
                                aiProcess_Triangulate |
                                aiProcess_JoinIdenticalVertices |
                                aiProcess_SortByPType);

    if(p_scene == nullptr)
    {
      std::cout << "Failed to load model. Assimp error: " << aiGetErrorString()
                << std::endl;

      return 0;
    }

    meshes.resize(p_scene->mNumMeshes);

    // Iterate over meshes.
    for(uint32_t mesh_num{0}; mesh_num < p_scene->mNumMeshes; mesh_num++)
    {
      const aiMesh* p_ai_mesh{p_scene->mMeshes[mesh_num]};

      meshes[mesh_num].vertex_base = vertexes.size();
      meshes[mesh_num].index_base = indexes.size();

      meshes[mesh_num].vertex_count = p_ai_mesh->mNumVertices;

      aiColor3D ai_color{0.0f, 0.0f, 0.0f};
      p_scene->mMaterials[p_ai_mesh->mMaterialIndex]->Get(
          AI_MATKEY_COLOR_DIFFUSE, ai_color);

      meshes[mesh_num].color.r = ai_color.r;
      meshes[mesh_num].color.g = ai_color.g;
      meshes[mesh_num].color.b = ai_color.b;

      const aiVector3D zero3d{0.0f, 0.0f, 0.0f};

      // FIXME: assimp is multiplicating vertexes.
      // Load vertexes.
      for(uint32_t vertex_num{0}; vertex_num < p_ai_mesh->mNumVertices;
          vertex_num++)
      {
        /*
          Get data from model.
        */

        const aiVector3D* p_position;
        const aiVector3D* p_normal;
        const aiVector3D* p_texture_coord;

        Vertex vert;

        p_position = &(p_ai_mesh->mVertices[vertex_num]);
        p_normal = &(p_ai_mesh->mNormals[vertex_num]);
        p_texture_coord =
            (p_ai_mesh->HasTextureCoords(0)) ?
            &(p_ai_mesh->mTextureCoords[0][vertex_num]) : &zero3d;

        /*
          Save data.
        */

        vert.position.x = p_position->x;
        vert.position.y = p_position->y;
        vert.position.z = p_position->z;

        vert.texture_coord.x = p_texture_coord->x;
        vert.texture_coord.y = p_texture_coord->y;

        vertexes.push_back(vert);
      }

      // Get number of vertices from previous meshes.
      uint32_t index_base{static_cast<uint32_t>(indexes.size())};

      // Load faces.
      for(uint32_t face_num{0}; face_num < p_ai_mesh->mNumFaces; face_num++)
      {
        const aiFace& ai_face{p_ai_mesh->mFaces[face_num]};

        // Ignore any polygon that is not a triangle.
        if (ai_face.mNumIndices != 3) continue;

        indexes.push_back(index_base + ai_face.mIndices[0]);
        indexes.push_back(index_base + ai_face.mIndices[1]);
        indexes.push_back(index_base + ai_face.mIndices[2]);

        meshes[mesh_num].index_count += 3;
      }
    }

    /*
      Print information about model.
    */

    std::cout << "Vertex count: " << vertexes.size() << std::endl;
    std::cout << "Index count: " << indexes.size() << std::endl;

    std::cout << "Meshes: " << meshes.size() << std::endl;
    for(auto mesh: meshes)
    {
      std::cout << "Color: r: " << mesh.color.r << ", g: " << mesh.color.g <<
          ", b:" << mesh.color.b << std::endl;

      std::cout << "Vertex base: " << mesh.vertex_base << std::endl;
      std::cout << "Vertex count: " << mesh.vertex_count << std::endl;

      std::cout << "Index base: " << mesh.index_base << std::endl;
      std::cout << "Index count: " << mesh.index_count << std::endl;

      std::cout << std::endl;
    }

    // Save meshes.
    {
      uint32_t meshes_count{meshes.size()};
      write_to_file(output_file, meshes_count);

      for(auto mesh: meshes)
      {
        write_to_file(output_file, mesh.color);

        write_to_file(output_file, mesh.vertex_base);
        write_to_file(output_file, mesh.vertex_count);
        write_to_file(output_file, mesh.index_base);
        write_to_file(output_file, mesh.index_count);
      }
    }

    // Save vertexes on file.
    {
      uint32_t vertex_count{vertexes.size()};
      write_to_file(output_file, vertex_count);

      for(auto vertex: vertexes)
      {
        write_to_file(output_file, vertex.position);
        write_to_file(output_file, vertex.normal);
        write_to_file(output_file, vertex.texture_coord);
      }
    }

    // Save indexes on file.
    {
      uint32_t index_count{indexes.size()};
      write_to_file(output_file, index_count);

      for(auto index: indexes) write_to_file(output_file, index);
    }
  }

  return 0;
}


/*
 Copyright (C) 2020 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 <qle/termstructures/spreadedblackvolatilitysurfacemoneyness.hpp>

#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/flatextrapolation2d.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <ql/utilities/dataformatters.hpp>

#include <boost/make_shared.hpp>

using namespace std;

namespace QuantExt {

SpreadedBlackVolatilitySurfaceMoneyness::SpreadedBlackVolatilitySurfaceMoneyness(
    const Handle<BlackVolTermStructure>& referenceVol, const Handle<Quote>& movingSpot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote>>>& volSpreads,
    const Handle<Quote>& stickySpot, const Handle<YieldTermStructure>& stickyDividendTs,
    const Handle<YieldTermStructure>& stickyRiskFreeTs, const Handle<YieldTermStructure>& movingDividendTs,
    const Handle<YieldTermStructure>& movingRiskFreeTs, bool stickyStrike)
    : BlackVolatilityTermStructure(referenceVol->businessDayConvention(), referenceVol->dayCounter()),
      referenceVol_(referenceVol), movingSpot_(movingSpot), times_(times), moneyness_(moneyness),
      volSpreads_(volSpreads), stickySpot_(stickySpot), stickyDividendTs_(stickyDividendTs),
      stickyRiskFreeTs_(stickyRiskFreeTs), movingDividendTs_(movingDividendTs), movingRiskFreeTs_(movingRiskFreeTs),
      stickyStrike_(stickyStrike) {

    // register with observables

    registerWith(referenceVol_);
    registerWith(movingSpot_);
    registerWith(stickySpot_);

    for (auto const& v : volSpreads_)
        for (auto const& s : v)
            registerWith(s);

    registerWith(stickyDividendTs_);
    registerWith(stickyRiskFreeTs_);
    registerWith(movingDividendTs_);
    registerWith(movingRiskFreeTs_);

    // check our preconditions on the inputs

    QL_REQUIRE(!times_.empty(), "no times given");
    QL_REQUIRE(!moneyness_.empty(), "no moneyness values given");
    QL_REQUIRE(moneyness_.size() == volSpreads_.size(), "mismatch between moneyness vector and vol matrix rows");

    for (auto const& v : volSpreads_) {
        QL_REQUIRE(times_.size() == v.size(), "mismatch between times vector and vol matrix colums");
    }

    for (Size j = 1; j < times_.size(); ++j) {
        QL_REQUIRE(times_[j] > times_[j - 1], "Times must be sorted and unique but found that the "
                                                  << io::ordinal(j) << " time, " << times_[j]
                                                  << ", is not greater than the " << io::ordinal(j - 1) << " time, "
                                                  << times_[j - 1] << ".");
    }

    // add an artificial time if there is only one to make the interpolation work

    if (times_.size() == 1) {
        times_.push_back(times_.back() + 1.0);
        for (auto& v : volSpreads_)
            v.push_back(v.back());
    }

    // add an artificial moneyness if there is only one to make the interpolation work

    if (moneyness_.size() == 1) {
        moneyness_.push_back(moneyness_.back() + 1.0);
        volSpreads_.push_back(volSpreads_.back());
    }

    // create data matrix used for interpolation and the interpolation object

    data_ = Matrix(moneyness_.size(), times_.size(), 0.0);
    volSpreadSurface_ = FlatExtrapolator2D(boost::make_shared<BilinearInterpolation>(
        times_.begin(), times_.end(), moneyness_.begin(), moneyness_.end(), data_));
    volSpreadSurface_.enableExtrapolation();
}

Date SpreadedBlackVolatilitySurfaceMoneyness::maxDate() const { return referenceVol_->maxDate(); }
const Date& SpreadedBlackVolatilitySurfaceMoneyness::referenceDate() const { return referenceVol_->referenceDate(); }
Calendar SpreadedBlackVolatilitySurfaceMoneyness::calendar() const { return referenceVol_->calendar(); }
Natural SpreadedBlackVolatilitySurfaceMoneyness::settlementDays() const { return referenceVol_->settlementDays(); }
Real SpreadedBlackVolatilitySurfaceMoneyness::minStrike() const { return referenceVol_->minStrike(); }
Real SpreadedBlackVolatilitySurfaceMoneyness::maxStrike() const { return referenceVol_->maxStrike(); }

void SpreadedBlackVolatilitySurfaceMoneyness::update() {
    LazyObject::update();
    BlackVolatilityTermStructure::update();
}

const std::vector<QuantLib::Real>& SpreadedBlackVolatilitySurfaceMoneyness::moneyness() const { return moneyness_; }

void SpreadedBlackVolatilitySurfaceMoneyness::performCalculations() const {
    for (Size j = 0; j < data_.columns(); ++j) {
        for (Size i = 0; i < data_.rows(); ++i) {
            data_(i, j) = volSpreads_[i][j]->value();
        }
    }
    volSpreadSurface_.update();
}

Real SpreadedBlackVolatilitySurfaceMoneyness::blackVolImpl(Time t, Real strike) const {
    calculate();
    QL_REQUIRE(!referenceVol_.empty(), "SpreadedBlackVolatilitySurfaceMoneyness: reference vol is empty");
    Real m = moneynessFromStrike(t, strike, false);
    QL_REQUIRE(std::isfinite(m),
               "SpreadedBlackVolatilitySurfaceMoneyness: got invalid moneyness (dynamic reference) at t = "
                   << t << ", strike = " << strike << ": " << m);
    Real effStrike;
    if (stickyStrike_)
        effStrike = strike;
    else {
        effStrike = strikeFromMoneyness(t, m, true);
        QL_REQUIRE(std::isfinite(effStrike),
                   "SpreadedBlackVolatilitySurfaceMoneyness: got invalid strike from moneyness at t = "
                       << t << ", input strike = " << strike << ", moneyness = " << m);
    }
    Real m2 = moneynessFromStrike(t, strike, false);
    QL_REQUIRE(std::isfinite(m2),
               "SpreadedBlackVolatilitySurfaceMoneyness: got invalid moneyness (sticky reference) at t = "
                   << t << ", strike = " << strike << ": " << m2);
    return referenceVol_->blackVol(t, effStrike) + volSpreadSurface_(t, m2);
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpot::moneynessFromStrike(Time t, Real strike,
                                                                      const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0)) {
        return 1.0;
    } else {
        QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
        QL_REQUIRE(stickyReference || !movingSpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
        return strike / (stickyReference ? stickySpot_->value() : movingSpot_->value());
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpot::strikeFromMoneyness(Time t, Real moneyness,
                                                                      const bool stickyReference) const {
    QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
    QL_REQUIRE(stickyReference || !movingSpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
    return moneyness * (stickyReference ? stickySpot_->value() : movingSpot_->value());
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessSpot::moneynessFromStrike(Time t, Real strike,
                                                                         const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0)) {
        return 0.0;
    } else {
        QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: stickySpot is empty");
        QL_REQUIRE(stickyReference || !movingSpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: movingSpot is empty");
        return std::log(strike / (stickyReference ? stickySpot_->value() : movingSpot_->value()));
    }
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessSpot::strikeFromMoneyness(Time t, Real moneyness,
                                                                         const bool stickyReference) const {
    QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
               "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: stickySpot is empty");
    QL_REQUIRE(stickyReference || !movingSpot_.empty(),
               "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: movingSpot is empty");
    return std::exp(moneyness) * (stickyReference ? stickySpot_->value() : movingSpot_->value());
}

Real SpreadedBlackVolatilitySurfaceMoneynessForward::moneynessFromStrike(Time t, Real strike,
                                                                         const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0))
        return 1.0;
    else {
        Real forward;
        if (stickyReference) {
            QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
            QL_REQUIRE(!stickyDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
            QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
            forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        return strike / forward;
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessForward::strikeFromMoneyness(Time t, Real moneyness,
                                                                         const bool stickyReference) const {
    Real forward;
    if (stickyReference) {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
        forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    return moneyness * forward;
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessForward::moneynessFromStrike(Time t, Real strike,
                                                                            const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0))
        return 0.0;
    else {
        Real forward;
        if (stickyReference) {
            QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickySpot is empty");
            QL_REQUIRE(!stickyDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyDividendTs is empty");
            QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyRiskFreeTs is empty");
            forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        return std::log(strike / forward);
    }
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessForward::strikeFromMoneyness(Time t, Real moneyness,
                                                                            const bool stickyReference) const {
    Real forward;
    if (stickyReference) {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyRiskFreeTs is empty");
        forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    return std::exp(moneyness) * forward;
}

Real SpreadedBlackVolatilitySurfaceStdDevs::moneynessFromStrike(Time t, Real strike, const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0) || close_enough(t, 0.0))
        return 0.0;
    else {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: stickyRiskFreeTs is empty");
        Real stickyForward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        Real forward;
        if (stickyReference) {
            forward = stickyForward;
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        // We use the sticky forward to read the vol for the definition of the standardised moneyness to
        // avoid that this changes under forward curve changes. In the end this is a matter of definition and
        // we might want to revise this later.
        Real vol = referenceVol_->blackVol(t, stickyForward);
        return std::log(strike / forward) / (vol * std::sqrt(t));
    }
}

Real SpreadedBlackVolatilitySurfaceStdDevs::strikeFromMoneyness(Time t, Real moneyness,
                                                                const bool stickyReference) const {
    Real stickyForward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    Real forward;
    if (stickyReference) {
        forward = stickyForward;
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    // We use the sticky forward to read the vol for the definition of the standardised moneyness to
    // avoid that this changes under forward curve changes. In the end this is a matter of definition and
    // we might want to revise this later.
    Real vol = referenceVol_->blackVol(t, stickyForward);
    return std::exp(moneyness * vol * std::sqrt(t)) * forward;
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpotAbsolute::moneynessFromStrike(Time t, Real strike,
                                                                              const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0)) {
        return 0.0;
    } else {
        QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
        QL_REQUIRE(stickyReference || !movingSpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
        return strike - (stickyReference ? stickySpot_->value() : movingSpot_->value());
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpotAbsolute::strikeFromMoneyness(Time t, Real moneyness,
                                                                              const bool stickyReference) const {
    QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
    QL_REQUIRE(stickyReference || !movingSpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
    return moneyness + (stickyReference ? stickySpot_->value() : movingSpot_->value());
}

Real SpreadedBlackVolatilitySurfaceMoneynessForwardAbsolute::moneynessFromStrike(Time t, Real strike,
                                                                                const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0))
        return 0.0;
    else {
        Real forward;
        if (stickyReference) {
            QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
            QL_REQUIRE(!stickyDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
            QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
            forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        return strike - forward;
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessForwardAbsolute::strikeFromMoneyness(Time t, Real moneyness,
                                                                                const bool stickyReference) const {
    Real forward;
    if (stickyReference) {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
        forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    return moneyness + forward;
}

} // namespace QuantExt


// Copyright © 2016 Venture Media Labs.
//
// This file is part of mxml. The full mxml copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include <mxml/EqualityConstraintSolver.h>
#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace mxml;

BOOST_AUTO_TEST_CASE(trivialCase) {
    EqualityConstraintSolver solver;
    solver.setVariableCount(1);
    auto count = solver.solve([](const std::vector<bool>& values) {
        BOOST_CHECK_EQUAL(values.size(), 1);
    });
    BOOST_CHECK_EQUAL(count, 2);
}

BOOST_AUTO_TEST_CASE(unsatisfyable) {
    EqualityConstraintSolver solver;
    solver.setVariableCount(3);
    solver.addDifferentConstraint(0, 1);
    solver.addDifferentConstraint(1, 2);
    solver.addDifferentConstraint(0, 2);

    auto count = solver.solve([](const std::vector<bool>& values) {
        BOOST_FAIL("There should not be a solution");
    });
    BOOST_CHECK_EQUAL(count, 0);
}

BOOST_AUTO_TEST_CASE(complex) {
    EqualityConstraintSolver solver;
    solver.setVariableCount(8);

    // A complex system with two beams and two solo notes:
    //
    //  0    1    2     7
    //  ┌────┬────┐
    //  │    │    │     │
    // ●    ●    ●     ●     ●
    //     │    │     │     │
    //          └─────┴─────┘
    //     3    4    5      6
    //

    // Beam: 0, 1, 2
    solver.addEqualConstraint(0, 1);
    solver.addEqualConstraint(1, 2);

    // Solo note: 3
    solver.addDifferentConstraint(1, 3);

    // Beam: 4, 5, 6
    solver.addEqualConstraint(4, 5);
    solver.addEqualConstraint(5, 6);
    solver.addEqualConstraint(4, 6);

    // Solo note: 7
    solver.addDifferentConstraint(5, 7);

    // Beam contact
    solver.addDifferentConstraint(2, 4);

    auto count = solver.solve([](const std::vector<bool>& values) {
        BOOST_CHECK_EQUAL(values[0], values[1]);
        BOOST_CHECK_EQUAL(values[1], values[2]);
        BOOST_CHECK_EQUAL(values[4], values[5]);
        BOOST_CHECK_EQUAL(values[5], values[6]);
        BOOST_CHECK_NE(values[1], values[3]);
        BOOST_CHECK_NE(values[2], values[4]);
        BOOST_CHECK_NE(values[5], values[7]);
    });

    // There should be two solutons: the one shown and it's opposite
    BOOST_CHECK_EQUAL(count, 2);
}


/*
 * Marginalizer.h
 *
 *  Created on: May 24, 2013
 *      Author: mbuerki
 */

#ifndef MARGINALIZER_H_
#define MARGINALIZER_H_

#include <aslam/backend/DesignVariable.hpp>
#include <aslam/backend/MarginalizationPriorErrorTerm.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <boost/shared_ptr.hpp>

namespace aslam {
namespace backend {

/// \brief Marginalizes out the given design variables
///
///	\param[IN] inDesignVariables list of input design variables to be marginalized
/// \param[IN] inErrorTerms list of all error terms related to the input design variables
/// \param[IN] numberOfInputDesignVariablesToRemove Number of input design variables to be removed.
/// 											The functions removes this many input design variables, starting at the beginning of
///												the list of input design variables. Therefore, the ordering of the design variables in the
///												input desing variables list matters!
/// \param[IN] useMEstimator					Wheter or not to use an M-Estimator in the QR sovler.
/// \param[OUT] outPriorErrorTermPtr			Shared pointer to the resulting marginalized prior error term.
///
void marginalize(
			std::vector<aslam::backend::DesignVariable*>& inDesignVariables,
			std::vector<aslam::backend::ErrorTerm*>& inErrorTerms,
			int numberOfInputDesignVariablesToRemove,
			bool useMEstimator,
			boost::shared_ptr<aslam::backend::MarginalizationPriorErrorTerm>& outPriorErrorTermPtr,
			Eigen::MatrixXd& outRtop,
			std::vector<aslam::backend::DesignVariable*>& designVariablesInvolvedInRtop,
			size_t numTopRowsInRtop = 0,
			size_t numThreads = 1
		);
} /* namespace backend */
} /* namespace aslam */
#endif /* MARGINALIZER_H_ */


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

// Boost.SpatialIndex - rtree implementation
//
// Copyright 2008 Federico J. Fernandez.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_INDEX_RTREE_RTREE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_INDEX_RTREE_RTREE_HPP

#include <cstddef>
#include <iostream> // TODO: Remove if print() is removed
#include <stdexcept>
#include <utility>
#include <vector>

#include <boost/concept_check.hpp>
#include <boost/shared_ptr.hpp>

#include <boost/geometry/algorithms/area.hpp>

#include <boost/geometry/extensions/index/rtree/rtree_node.hpp>
#include <boost/geometry/extensions/index/rtree/rtree_leaf.hpp>

namespace boost { namespace geometry { namespace index
{

template <typename Box, typename Value >
class rtree
{
public:

    typedef boost::shared_ptr<rtree_node<Box, Value> > node_pointer;
    typedef boost::shared_ptr<rtree_leaf<Box, Value> > leaf_pointer;

    /**
     * \brief Creates a rtree with 'maximum' elements per node and 'minimum'.
     */
    rtree(unsigned int const& maximum, unsigned int const& minimum)
        : m_count(0)
        , m_min_elems_per_node(minimum)
        , m_max_elems_per_node(maximum)
        , m_root(new rtree_node<Box, Value>(node_pointer(), 1))
    {
    }

    /**
     * \brief Creates a rtree with maximum elements per node
     *        and minimum (box is ignored).
     */
    rtree(Box const& box, unsigned int const& maximum, unsigned int const& minimum)
        : m_count(0)
        , m_min_elems_per_node(minimum)
        , m_max_elems_per_node(maximum)
        , m_root(new rtree_node<Box, Value>(node_pointer(), 1))
    {
        boost::ignore_unused_variable_warning(box);
    }

    /**
     * \brief destructor (virtual because we have virtual functions)
     */
    virtual ~rtree() {}

    /**
     * \brief Remove elements inside the 'box'
     */
    inline void remove(Box const& box)
    {
        try
        {
            node_pointer leaf(choose_exact_leaf(box));
            typename rtree_leaf<Box, Value>::leaf_map q_leaves;

            leaf->remove(box);

            if (leaf->elements() < m_min_elems_per_node && elements() > m_min_elems_per_node)
            {
                q_leaves = leaf->get_leaves();

                // we remove the leaf_node in the parent node because now it's empty
                leaf->get_parent()->remove(leaf->get_parent()->get_box(leaf));
            }

            typename rtree_node<Box, Value>::node_map q_nodes;
            condense_tree(leaf, q_nodes);

            std::vector<std::pair<Box, Value> > s;
            for (typename rtree_node<Box, Value>::node_map::const_iterator it = q_nodes.begin();
                 it != q_nodes.end(); ++it)
            {
                typename rtree_leaf<Box, Value>::leaf_map leaves = it->second->get_leaves();

                // reinserting leaves from nodes
                for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator itl = leaves.begin();
                     itl != leaves.end(); ++itl)
                {
                    s.push_back(*itl);
                }
            }

            for (typename std::vector<std::pair<Box, Value> >::const_iterator it = s.begin(); it != s.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            // if the root has only one child and the child is not a leaf,
            // make it the root
            if (m_root->elements() == 1)
            {
                if (!m_root->first_element()->is_leaf())
                {
                    m_root = m_root->first_element();
                }
            }
            // reinserting leaves
            for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator it = q_leaves.begin();
                 it != q_leaves.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            m_count--;
        }
        catch(std::logic_error & e)
        {
            // TODO: mloskot - replace with Boost.Geometry exception

            // not found
            std::cerr << e.what() << std::endl;
            return;
        }
    }

    /**
     * \brief Remove element inside the box with value
     */
    void remove(Box const& box, Value const& value)
    {
        try
        {
            node_pointer leaf;

            // find possible leaves
            typedef typename std::vector<node_pointer > node_type;
            node_type nodes;
            m_root->find_leaves(box, nodes);

            // refine the result
            for (typename node_type::const_iterator it = nodes.begin(); it != nodes.end(); ++it)
            {
                leaf = *it;
                try
                {
                    leaf->remove(value);
                    break;
                } catch (...)
                {
                    leaf = node_pointer();
                }
            }

            if (!leaf)
                return;

            typename rtree_leaf < Box, Value >::leaf_map q_leaves;

            if (leaf->elements() < m_min_elems_per_node && elements() > m_min_elems_per_node)
            {
                q_leaves = leaf->get_leaves();

                // we remove the leaf_node in the parent node because now it's empty
                leaf->get_parent()->remove(leaf->get_parent()->get_box(leaf));
            }

            typename rtree_node<Box, Value>::node_map q_nodes;
            condense_tree(leaf, q_nodes);

            std::vector<std::pair<Box, Value> > s;
            for (typename rtree_node<Box, Value>::node_map::const_iterator it = q_nodes.begin();
                 it != q_nodes.end(); ++it)
            {
                typename rtree_leaf<Box, Value>::leaf_map leaves = it->second->get_leaves();

                // reinserting leaves from nodes
                for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator itl = leaves.begin();
                     itl != leaves.end(); ++itl)
                {
                    s.push_back(*itl);
                }
            }

            for (typename std::vector<std::pair<Box, Value> >::const_iterator it = s.begin(); it != s.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            // if the root has only one child and the child is not a leaf,
            // make it the root
            if (m_root->elements() == 1)
            {
                if (!m_root->first_element()->is_leaf())
                {
                    m_root = m_root->first_element();
                }
            }

            // reinserting leaves
            for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator it = q_leaves.begin();
                 it != q_leaves.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            m_count--;

        }
        catch(std::logic_error & e)
        {
            // TODO: mloskot - ggl exception

            // not found
            std::cerr << e.what() << std::endl;
            return;
        }
    }

    /**
     * \brief Returns the number of elements.
     */
    inline unsigned int elements() const
    {
        return m_count;
    }

    /**
     * \brief Inserts an element with 'box' as key with value.
     */
    inline void insert(Box const& box, Value const& value)
    {
        m_count++;

        node_pointer leaf(choose_corresponding_leaf(box));

        // check if the selected leaf is full to do the split if necessary
        if (leaf->elements() >= m_max_elems_per_node)
        {
            leaf->insert(box, value);

            // split!
            node_pointer n1(new rtree_leaf<Box, Value>(leaf->get_parent()));
            node_pointer n2(new rtree_leaf<Box, Value>(leaf->get_parent()));

            split_node(leaf, n1, n2);
            adjust_tree(leaf, n1, n2);
        }
        else
        {
            leaf->insert(box, value);
            adjust_tree(leaf);
        }
    }

    /**
     * \brief Returns all the values inside 'box'
     */
    inline std::deque<Value> find(Box const& box) const
    {
        std::deque<Value> result;
        m_root->find(box, result, false);
        return result;
    }

    /**
     * \brief Print Rtree (mainly for debug)
     */
    inline void print()
    {
        std::cerr << "===================================" << std::endl;
        std::cerr << " Min/Max: " << m_min_elems_per_node << " / " << m_max_elems_per_node << std::endl;
        std::cerr << "Leaves: " << m_root->get_leaves().size() << std::endl;
        m_root->print();
        std::cerr << "===================================" << std::endl;
    }

private:

    /// number of elements
    unsigned int m_count;

    /// minimum number of elements per node
    unsigned int m_min_elems_per_node;

    /// maximum number of elements per node
    unsigned int m_max_elems_per_node;

    /// tree root
    node_pointer m_root;

    /**
     * \brief Reorganize the tree after a removal. It tries to
     *        join nodes with less elements than m.
     */
    void condense_tree(node_pointer const& leaf,
        typename rtree_node<Box, Value>::node_map& q_nodes)
    {
        if (leaf.get() == m_root.get())
        {
            // if it's the root we are done
            return;
        }

        node_pointer parent = leaf->get_parent();
        parent->adjust_box(leaf);

        if (parent->elements() < m_min_elems_per_node)
        {
            if (parent.get() == m_root.get())
            {
                // if the parent is underfull and it's the root we just exit
                return;
            }

            // get the nodes that we should reinsert
            typename rtree_node<Box, Value>::node_map this_nodes = parent->get_nodes();
            for(typename rtree_node<Box, Value>::node_map::const_iterator it = this_nodes.begin();
                it != this_nodes.end(); ++it)
            {
                q_nodes.push_back(*it);
            }

            // we remove the node in the parent node because now it should be
            // re inserted
            parent->get_parent()->remove(parent->get_parent()->get_box(parent));
        }

        condense_tree(parent, q_nodes);
    }

    /**
     * \brief After an insertion splits nodes with more than 'maximum' elements.
     */
    inline void adjust_tree(node_pointer& node)
    {
        if (node.get() == m_root.get())
        {
            // we finished the adjust
            return;
        }

        // as there are no splits just adjust the box of the parent and go on
        node_pointer parent = node->get_parent();
        parent->adjust_box(node);
        adjust_tree(parent);
    }

    /**
     * \brief After an insertion splits nodes with more than maximum elements
     *        (recursive step with subtrees 'n1' and 'n2' to be joined).
     */
    void adjust_tree(node_pointer& leaf, node_pointer& n1, node_pointer& n2)
    {
        // check if we are in the root and do the split
        if (leaf.get() == m_root.get())
        {
            node_pointer new_root(new rtree_node<Box,Value>(node_pointer (), leaf->get_level() + 1));
            new_root->add_node(n1->compute_box(), n1);
            new_root->add_node(n2->compute_box(), n2);

            n1->set_parent(new_root);
            n2->set_parent(new_root);

            n1->update_parent(n1);
            n2->update_parent(n2);

            m_root = new_root;
            return;
        }

        node_pointer parent = leaf->get_parent();

        parent->replace_node(leaf, n1);
        parent->add_node(n2->compute_box(), n2);

        // if parent is full, split and readjust
        if (parent->elements() > m_max_elems_per_node)
        {
            node_pointer p1(new rtree_node<Box, Value>(parent->get_parent(), parent->get_level()));
            node_pointer p2(new rtree_node<Box, Value>(parent->get_parent(), parent->get_level()));

            split_node(parent, p1, p2);
            adjust_tree(parent, p1, p2);
        }
        else
        {
            adjust_tree(parent);
        }
    }

    /**
     * \brief Splits 'n' in 'n1' and 'n2'
     */
    void split_node(node_pointer const& n, node_pointer& n1, node_pointer& n2) const
    {
        unsigned int seed1 = 0;
        unsigned int seed2 = 0;
        std::vector<Box> boxes = n->get_boxes();

        n1->set_parent(n->get_parent());
        n2->set_parent(n->get_parent());

        linear_pick_seeds(n, seed1, seed2);

        if (n->is_leaf())
        {
            n1->add_value(boxes[seed1], n->get_value(seed1));
            n2->add_value(boxes[seed2], n->get_value(seed2));
        }
        else
        {
            n1->add_node(boxes[seed1], n->get_node(seed1));
            n2->add_node(boxes[seed2], n->get_node(seed2));
        }

        unsigned int index = 0;

        if (n->is_leaf())
        {
            // TODO: mloskot - add BOOST_GEOMETRY_ASSERT(node.size() >= 2); or similar

            typename rtree_leaf<Box, Value>::leaf_map nodes = n->get_leaves();
            unsigned int remaining = nodes.size() - 2;

            for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator it = nodes.begin();
                 it != nodes.end(); ++it, index++)
            {
                if (index != seed1 && index != seed2)
                {
                    if (n1->elements() + remaining == m_min_elems_per_node)
                    {
                        n1->add_value(it->first, it->second);
                        continue;
                    }
                    if (n2->elements() + remaining == m_min_elems_per_node)
                    {
                        n2->add_value(it->first, it->second);
                        continue;
                    }

                    remaining--;

                    /// current boxes of each group
                    Box b1, b2;

                    /// enlarged boxes of each group
                    Box eb1, eb2;
                    b1 = n1->compute_box();
                    b2 = n2->compute_box();

                    /// areas
                    typedef typename coordinate_type<Box>::type coordinate_type;
                    coordinate_type b1_area, b2_area;
                    coordinate_type eb1_area, eb2_area;
                    b1_area = geometry::area(b1);
                    b2_area = geometry::area(b2);
                    eb1_area = compute_union_area(b1, it->first);
                    eb2_area = compute_union_area(b2, it->first);

                    if (eb1_area - b1_area > eb2_area - b2_area)
                    {
                        n2->add_value(it->first, it->second);
                    }
                    if (eb1_area - b1_area < eb2_area - b2_area)
                    {
                        n1->add_value(it->first, it->second);
                    }
                    if (eb1_area - b1_area == eb2_area - b2_area)
                    {
                        if (b1_area < b2_area)
                        {
                            n1->add_value(it->first, it->second);
                        }
                        if (b1_area > b2_area)
                        {
                            n2->add_value(it->first, it->second);
                        }
                        if (b1_area == b2_area)
                        {
                            if (n1->elements() > n2->elements())
                            {
                                n2->add_value(it->first, it->second);
                            }
                            else
                            {
                                n1->add_value(it->first, it->second);
                            }
                        }
                    }
                }
            }
        }
        else
        {
            // TODO: mloskot - add BOOST_GEOMETRY_ASSERT(node.size() >= 2); or similar

            typename rtree_node<Box, Value>::node_map nodes = n->get_nodes();
            unsigned int remaining = nodes.size() - 2;
            for(typename rtree_node<Box, Value>::node_map::const_iterator it = nodes.begin();
                it != nodes.end(); ++it, index++)
            {

                if (index != seed1 && index != seed2)
                {

                    if (n1->elements() + remaining == m_min_elems_per_node)
                    {
                        n1->add_node(it->first, it->second);
                        continue;
                    }
                    if (n2->elements() + remaining == m_min_elems_per_node)
                    {
                        n2->add_node(it->first, it->second);
                        continue;
                    }

                    remaining--;

                    /// current boxes of each group
                    Box b1, b2;

                    /// enlarged boxes of each group
                    Box eb1, eb2;
                    b1 = n1->compute_box();
                    b2 = n2->compute_box();

                    /// areas
                    typedef typename coordinate_type<Box>::type coordinate_type;
                    coordinate_type b1_area, b2_area;
                    coordinate_type eb1_area, eb2_area;
                    b1_area = geometry::area(b1);
                    b2_area = geometry::area(b2);

                    eb1_area = compute_union_area(b1, it->first);
                    eb2_area = compute_union_area(b2, it->first);

                    if (eb1_area - b1_area > eb2_area - b2_area)
                    {
                        n2->add_node(it->first, it->second);
                    }
                    if (eb1_area - b1_area < eb2_area - b2_area)
                    {
                        n1->add_node(it->first, it->second);
                    }
                    if (eb1_area - b1_area == eb2_area - b2_area)
                    {
                        if (b1_area < b2_area)
                        {
                            n1->add_node(it->first, it->second);
                        }
                        if (b1_area > b2_area)
                        {
                            n2->add_node(it->first, it->second);
                        }
                        if (b1_area == b2_area)
                        {
                            if (n1->elements() > n2->elements())
                            {
                                n2->add_node(it->first, it->second);
                            }
                            else
                            {
                                n1->add_node(it->first, it->second);
                            }
                        }
                    }

                }
            }
        }
    }

    /**
     * \brief Choose initial values for the split algorithm (linear version)
     */
    void linear_pick_seeds(node_pointer const& n, unsigned int &seed1, unsigned int &seed2) const
    {
        // get boxes from the node
        std::vector<Box>boxes = n->get_boxes();
        if (boxes.size() == 0)
        {
            // TODO: mloskot - throw ggl exception
            throw std::logic_error("Empty Node trying to Pick Seeds");
        }

        // only two dim for now
        // unsigned int dimensions =
        //   geometry::point_traits<Point>::coordinate_count;

        // find the first two elements
        typedef typename coordinate_type<Box>::type coordinate_type;
        coordinate_type separation_x, separation_y;
        unsigned int first_x, second_x;
        unsigned int first_y, second_y;
        find_normalized_separations<0u>(boxes, separation_x, first_x, second_x);
        find_normalized_separations<1u>(boxes, separation_y, first_y, second_y);

        if (separation_x > separation_y)
        {
            seed1 = first_x;
            seed2 = second_x;
        }
        else
        {
            seed1 = first_y;
            seed2 = second_y;
        }
    }

    /**
     * \brief Find distances between possible initial values for the
     *        pick_seeds algorithm.
     */
    template <std::size_t D, typename T>
    void find_normalized_separations(std::vector<Box> const& boxes, T& separation,
        unsigned int& first, unsigned int& second) const
    {
        if (boxes.size() < 2)
        {
            throw std::logic_error("At least two boxes needed to split");
        }

        // find the lowest high
        typename std::vector<Box>::const_iterator it = boxes.begin();
        typedef typename coordinate_type<Box>::type coordinate_type;
        coordinate_type lowest_high = geometry::get<max_corner, D>(*it);
        unsigned int lowest_high_index = 0;
        unsigned int index = 1;
        ++it;
        for(; it != boxes.end(); ++it)
        {
            if (geometry::get<max_corner, D>(*it) < lowest_high)
            {
                lowest_high = geometry::get<max_corner, D>(*it);
                lowest_high_index = index;
            }
            index++;
        }

        // find the highest low
        coordinate_type highest_low = 0;
        unsigned int highest_low_index = 0;
        if (lowest_high_index == 0)
        {
            highest_low = geometry::get<min_corner, D>(boxes[1]);
            highest_low_index = 1;
        }
        else
        {
            highest_low = geometry::get<min_corner, D>(boxes[0]);
            highest_low_index = 0;
        }

        index = 0;
        for (typename std::vector<Box>::const_iterator it = boxes.begin();
             it != boxes.end(); ++it, index++)
        {
            if (geometry::get<min_corner, D>(*it) >= highest_low && index != lowest_high_index)
            {
                highest_low = geometry::get<min_corner, D>(*it);
                highest_low_index = index;
            }
        }

        // find the lowest low
        it = boxes.begin();
        coordinate_type lowest_low = geometry::get<min_corner, D>(*it);
        ++it;
        for(; it != boxes.end(); ++it)
        {
            if (geometry::get<min_corner, D>(*it) < lowest_low)
            {
                lowest_low = geometry::get<min_corner, D>(*it);
            }
        }

        // find the highest high
        it = boxes.begin();
        coordinate_type highest_high = geometry::get<max_corner, D>(*it);
        ++it;
        for(; it != boxes.end(); ++it)
        {
            if (geometry::get<max_corner, D>(*it) > highest_high)
            {
                highest_high = geometry::get<max_corner, D>(*it);
            }
        }

        coordinate_type const width = highest_high - lowest_low;

        separation = (highest_low - lowest_high) / width;
        first = highest_low_index;
        second = lowest_high_index;
    }

    /**
     * \brief Choose one of the possible leaves to make an insertion
     */
    inline node_pointer choose_corresponding_leaf(Box const& e)
    {
        node_pointer node = m_root;

        // if the tree is empty add an initial leaf
        if (m_root->elements() == 0)
        {
            leaf_pointer new_leaf(new rtree_leaf<Box, Value>(m_root));
            m_root->add_leaf_node(Box (), new_leaf);

            return new_leaf;
        }

        while (!node->is_leaf())
        {
            /// traverse node's map to see which node we should select
            node = node->choose_node(e);
        }
        return node;
    }

    /**
     * \brief Choose the exact leaf where an insertion should be done
     */
    node_pointer choose_exact_leaf(Box const&e) const
    {
        // find possible leaves
        typedef typename std::vector<node_pointer> node_type;
        node_type nodes;
        m_root->find_leaves(e, nodes);

        // refine the result
        for (typename node_type::const_iterator it = nodes.begin(); it != nodes.end(); ++it)
        {
            typedef std::vector<std::pair<Box, Value> > leaves_type;
            leaves_type leaves = (*it)->get_leaves();

            for (typename leaves_type::const_iterator itl = leaves.begin();
                 itl != leaves.end(); ++itl)
            {

                if (itl->first.max_corner() == e.max_corner()
                    && itl->first.min_corner() == e.min_corner())
                {
                    return *it;
                }
            }
        }

        // TODO: mloskot - ggl exception
        throw std::logic_error("Leaf not found");
    }
};

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_EXTENSIONS_INDEX_RTREE_RTREE_HPP


#ifndef BOOST_NETWORK_UTILS_BASE64_STATEFUL_ITERATOR_HPP
#define BOOST_NETWORK_UTILS_BASE64_STATEFUL_ITERATOR_HPP

#include <boost/archive/iterators/base64_from_binary.hpp>
#include "iterators/stateful_transform_width.hpp"
#include "iterators/iterator_with_state.hpp"
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/array.hpp>
#include <algorithm>
#include <iterator>
#include <sstream>
#include <string>

namespace boost {
namespace network {
namespace utils {

// Uses base64_from_binary and stateful_transform_width to implement a
// BASE64
// converter working on an iterator range.  Based on transform_width, the
// stateful_transform_width relies on the end iterator and the encoding
// state
// stored in the transformed iterator; usually by using the iterator
// adaptor
// - iterator_with_state.  Storing the encoding state in the transformed
// iterator practically means adapting every such iterator, but the
// iterators
// which wrap the stateful_transform_width, like the base64_from_binary,
// do
// not need to be re-declared with an extra constructor to pass the end
// iterator and the encoding state to an extended transform_width.
//
// Summarized interface:
//
// struct state<Value>  {
//     bool empty () const;
//     void clear();
//     unsigned short padding_length() const;
// }
//
// OutputIterator encode(InputIterator begin, InputIterator end,
//                       OutputIterator output, State & rest)
// OutputIterator encode_rest(OutputIterator output, State & rest)
// OutputIterator encode(InputRange const & input, OutputIterator output,
//                       State & rest)
// OutputIterator encode(char const * value, OutputIterator output,
//                       state<char> & rest)
// std::basic_string<Char> encode(InputRange const & value, State & rest)
// std::basic_string<Char> encode(char const * value, state<char> & rest)
//
// OutputIterator encode(InputIterator begin, InputIterator end,
//                       OutputIterator output)
// OutputIterator encode(InputRange const & input, OutputIterator output)
// OutputIterator encode(char const * value, OutputIterator output)
// std::basic_string<Char> encode(InputRange const & value)
// std::basic_string<Char> encode(char const * value) {

namespace base64_stateful_iterator {

// force using the ostream_iterator from boost::archive to write wide
// characters reliably, althoth wchar_t may not be a native character
// type
using namespace boost::archive::iterators;
using namespace boost::network::utils::iterators;

template <typename Value>
struct state : public state_for_transform_width<Value, 6, 8> {
  typedef state<Value> this_t;
  typedef state_for_transform_width<Value, 6, 8> super_t;

  state() {}
  state(this_t const& source) : super_t(source) {}

  unsigned short padding_length() const {
    // the BASE64 encoding unit consists of 6 bits; if the 8-bit
    // input cannot be completely divided to 6-bit chunks, 2 or
    // 4 bits can remain, which the bit_count returns as displacement
    // of the 8-bit value (right shift length) - 6 or 4
    unsigned short bits = super_t::bit_count();
    return bits > 0 ? 6 / bits : 0;
  }
};

template <typename InputIterator, typename OutputIterator, typename State>
OutputIterator encode(InputIterator begin, InputIterator end,
                      OutputIterator output, State& rest) {
  typedef boost::iterator_with_state<InputIterator, State> stateful_input;
  // declare the encoding iterator type
  typedef base64_from_binary<stateful_transform_width<stateful_input, 6, 8> >
      base64_input;
  // declare the stateful transforming itarators
  stateful_input stateful_begin(begin, end, rest), stateful_end(end);
  // declare the iterator to transform the encoded 6-bit units to the
  // BASE64 alphabet
  base64_input base64_begin(stateful_begin), base64_end(stateful_end);
  return std::copy(base64_begin, base64_end, output);
}

template <typename State, typename OutputIterator>
OutputIterator encode_rest(OutputIterator output, State& rest) {
  unsigned short padding_length = rest.padding_length();
  if (padding_length > 0) {
    typedef typename State::value_type value_type;
    // declare the input padding type and the adapted iterator including
    // the encoding state for it - see below
    typedef boost::array<value_type, 2> pillow_input;
    typedef boost::iterator_with_state<typename pillow_input::const_iterator,
                                       State> stateful_input;
    // declare the encoding iterator type
    typedef base64_from_binary<stateful_transform_width<stateful_input, 6, 8> >
        base64_input;
    // although containing encoding state to continue with the next
    // chunk, the stateful_transform_width still reads from the input
    // iterator and needs the complete quantum for the encoding; the
    // zero padding will make for an artifitial input ending here
    pillow_input pillow = {{0, 0}};
    stateful_input stateful_begin(pillow.begin(), pillow.end(), rest);
    base64_input base64_begin(stateful_begin);
    *output++ = *base64_begin;
    if (padding_length > 0) {
      *output++ = '=';
      if (padding_length > 1) *output++ = '=';
    }
  }
  return output;
}

template <typename InputIterator, typename OutputIterator>
OutputIterator encode(InputIterator begin, InputIterator end,
                      OutputIterator output) {
  state<typename iterator_value<InputIterator>::type> rest;
  output = encode(begin, end, output, rest);
  return encode_rest(output, rest);
}

template <typename InputRange, typename OutputIterator>
OutputIterator encode(InputRange const& input, OutputIterator output) {
  return encode(boost::begin(input), boost::end(input), output);
}

template <typename OutputIterator>
OutputIterator encode(char const* value, OutputIterator output) {
  return encode(value, value + strlen(value), output);
}

template <typename Char, typename InputRange>
std::basic_string<Char> encode(InputRange const& value) {
  std::basic_string<Char> result;
  encode(value, std::back_inserter(result));
  return result;
}

template <typename Char>
std::basic_string<Char> encode(char const* value) {
  std::basic_string<Char> result;
  encode(value, std::back_inserter(result));
  return result;
}

}  // namespace base64_stateful_iterator

}  // namespace utils
}  // namespace network
}  // namespace boost

#endif  // BOOST_NETWORK_UTILS_BASE64_STATEFUL_ITERATOR_HPP


/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <cmath>
#include <future>

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/opencv.hpp>
#include <unsupported/Eigen/CXX11/Tensor>

#include <boost/format.hpp>

#include "source/gpu/GlfwUtil.h"
#include "source/gpu/ReprojectionGpuUtil.h"
#include "source/util/Camera.h"
#include "source/util/CvUtil.h"
#include "source/util/ImageUtil.h"
#include "source/util/SystemUtil.h"

using namespace fb360_dep;

const std::string kUsageMessage = R"(
- Compute initial depth for every camera
- Repeat pass_count times:
  - Clean away depths that are implausible
  - Recompute depths using clean depths to estimate occlusions

- Example:
    GeometricConsistency \
    --color /path/to/color \
    --output /path/to/output \
    --rig /path/to/rigs/rig.json \
    --first 000000 \
    --last 000000
)";

DEFINE_double(agree_fraction, 0.75, "fraction considered in agreement");
DEFINE_string(color, "", "color directory (required)");
DEFINE_double(disparity_step, 0.5, "pixels per disparity step");
DEFINE_double(downscale, 4, "reduced resolution output");
DEFINE_string(first, "", "first frame to process (lexical)");
DEFINE_bool(keep_clean, false, "only recompute implausible depths");
DEFINE_string(last, "", "last frame to process (lexical)");
DEFINE_int32(median, 0, "radius of median filter applied to input");
DEFINE_string(output, "", "output subdirectory (required)");
DEFINE_int32(pass_count, 2, "how many times to refine depth");
DEFINE_string(rig, "", "path to rig .json file (required)");
DEFINE_string(single, "", "render a single destination camera");

// Image is 16-bit RGBA
using Pixel = cv::Vec4w;
using Image = cv::Mat_<Pixel>;

const int kSignedMax = INT16_MAX;
using SignedPixel = cv::Vec<int16_t, 4>;

void dump(const filesystem::path& path, const cv::Mat_<float>& mat) {
  fb360_dep::cv_util::writeCvMat32FC1ToPFM(path.string() + ".pfm", mat);
  // for convenience, also dump 1.0 m / mat as png
  cv::Mat_<float> disparity = 1.0 / mat;
  cv_util::imwriteExceptionOnFail(
      path.string() + "_disparity.png", cv_util::convertTo<uint16_t>(disparity));
}

// Depth is fp32
using DepthMat = cv::Mat_<float>;

template <typename T>
std::vector<GLuint> createTextures(
    const std::vector<T>& mats,
    const GLenum internalFormat, // e.g. GL_RGB8, GL_SRGB8_ALPHA8, GL_RGBA16F
    const GLenum format, // e.g. GL_RED, GL_RGB, GL_BGRA
    const GLenum type) { // e.g. GL_UNSIGNED_BYTE, GL_FLOAT
  std::vector<GLuint> result;
  for (const T& mat : mats) {
    result.push_back(createTexture(
        mat.cols,
        mat.rows,
        mat.ptr(),
        internalFormat,
        format,
        type,
        true)); // buildMipmaps = true
    setTextureAniso();
    setTextureWrap(GL_CLAMP_TO_BORDER);
  }
  return result;
}

static Camera::Real computeRigRadius(const Camera::Rig& rig) {
  Camera::Real sum = 0;
  for (const Camera& camera : rig) {
    sum += camera.position.norm();
  }
  return sum / rig.size();
}

template <typename T>
T sq(const T& t) {
  return t * t;
}

float sumNorm(const SignedPixel& rgba) {
  float sum = 0;
  for (int c = 0; c < 3; ++c) { // exclude alpha
    sum += float(rgba[c]);
  }
  return sum / kSignedMax;
}

float sumSqNorm(const SignedPixel& rgba) {
  float sum = 0;
  for (int c = 0; c < 3; ++c) { // exclude alpha
    sum += sq(float(rgba[c]));
  }
  return sum / (kSignedMax * kSignedMax);
}

Camera::Real sliceDisparity(const int slice, const int sliceCount) {
  return ReprojectionTable::unnormalizeDisparity((slice + 0.5) / sliceCount);
}

using Volume = Eigen::Tensor<float, 3, Eigen::RowMajor>;

DepthMat winnerTakesAll(const Volume& costs) {
  // find cheapest depth for each x, y in costs (NAN if everything is NAN)
  DepthMat depth(costs.dimension(1), costs.dimension(2), NAN);
  Eigen::MatrixXf best(depth.rows, depth.cols);
  best.setConstant(FLT_MAX);
  for (int d = 0; d < costs.dimension(0); ++d) {
    for (int y = 0; y < costs.dimension(1); ++y) {
      for (int x = 0; x < costs.dimension(2); ++x) {
        if (best(y, x) > costs(d, y, x)) { // note: (x > NAN) is alwyas false
          best(y, x) = costs(d, y, x);
          depth(y, x) = 1 / sliceDisparity(d, costs.dimension(0));
        }
      }
    }
  }

  // NAN out the edges
  for (int y = 0; y < costs.dimension(1); ++y) {
    depth(y, 0) = depth(y, costs.dimension(2) - 1) = NAN;
  }
  for (int x = 0; x < costs.dimension(2); ++x) {
    depth(0, x) = depth(costs.dimension(1) - 1, x) = NAN;
  }
  return depth;
}

cv::Mat_<SignedPixel> computeReference(const Image& image, int w, int h) {
  Image resized = cv_util::resizeImage(image, {w, h});
  cv::Mat_<SignedPixel> reference;
  resized.convertTo(reference, CV_16S, INT16_MAX / double(UINT16_MAX));
  return reference;
}

DepthMat computeDepth(
    const Camera::Rig& rig, // rig determines the resolution of the result
    const int d,
    const std::vector<Image>& images, // full resolution images
    const std::vector<GLuint>& imageTextures,
    const std::vector<DepthMat>& depths = {}, // could be same rez as rig, or not
    const std::vector<GLuint>& depthTextures = {}) {
  const Camera& dst = rig[d];
  LOG(INFO) << boost::format("compute depth for %1%") % dst.id;

  // compute reprojection textures
  std::vector<ReprojectionTexture> reprojections;
  for (const Camera& src : rig) {
    reprojections.emplace_back(dst, src);
  }

  // downsample destination image
  const int w = dst.resolution.x();
  const int h = dst.resolution.y();
  cv::Mat_<SignedPixel> reference = computeReference(images[d], w, h);

  // compute how many slices we need
  const Camera::Real radius = computeRigRadius(rig);
  const Camera::Real minDistance = 1 / ReprojectionTable::maxDisparity();
  const Camera::Real angle = asin(radius / minDistance);
  const Camera::Real focal = dst.focal.norm() * sqrt(0.5);
  const Camera::Real pixels = focal * angle;
  const int sliceCount = std::round(pixels / FLAGS_disparity_step);

  // compute each slice of the cost volume
  Volume costs(sliceCount, h, w);
  for (int slice = 0; slice < sliceCount; ++slice) {
    Camera::Real disparity = sliceDisparity(slice, sliceCount);
    LOG(INFO) << boost::format("slice %1%/%2% (%3%)") % slice % sliceCount % disparity;

    // accumulate each source cost into accum
    cv::Mat_<cv::Vec2f> accum(h, w, cv::Vec2f(0, 0));
    for (int s = 0; s < int(rig.size()); ++s) {
      if (s == d) {
        continue; // don't compare destination to itself
      }

      // compute src color at disparity by reprojection
      cv::Mat_<SignedPixel> image = reproject<SignedPixel>(
          w, h, GL_RGBA16, GL_RGBA, GL_SHORT, reprojections[s], imageTextures[s], disparity);

      // alpha away occluded areas if we have depth information
      if (!depths.empty()) {
        DepthMat depth = reproject<float>(
            w, h, GL_R32F, GL_RED, GL_FLOAT, reprojections[s], depthTextures[s], disparity);
        for (int y = 0; y < h; ++y) {
          for (int x = 0; x < w; ++x) {
            if (!std::isnan(depth(y, x))) {
              const Camera::Vector3 world = dst.rig({x, y}, 1 / disparity);
              const Camera::Real distance = (world - rig[s].position).norm();
              if (depth(y, x) < distance * FLAGS_agree_fraction) {
                image(y, x)[3] = 0; // src is occluded
              }
            }
          }
        }
      }

      // compute average of difference
      cv::Mat_<SignedPixel> diff = image - reference;
      const cv::Size box(3, 3);
      cv::Mat_<SignedPixel> average;
      cv::blur(diff, average, box);

      // compute average of diff^2
      cv::Mat_<SignedPixel> averageOfSq;
      cv::blur(diff.mul(diff, 1.0 / kSignedMax), averageOfSq, box);

      // cost += variance of diff = average of diff^2 - (average of diff)^2
      for (int y = 0; y < h; ++y) {
        for (int x = 0; x < w; ++x) {
          if (image(y, x)[3] == kSignedMax && average(y, x)[3] == 0) {
            accum(y, x) += cv::Vec2f(sumNorm(averageOfSq(y, x)) - sumSqNorm(average(y, x)), 1);
          }
        }
      }
    }

    // transfer accumulated fraction to cost
    for (int y = 0; y < h; ++y) {
      for (int x = 0; x < w; ++x) {
        costs(slice, y, x) = accum(y, x)[0] / accum(y, x)[1];
      }
    }
  }

  // winner takes all
  return winnerTakesAll(costs);
}

bool isPointBad(
    const Camera::Vector3& world,
    const Camera::Rig& rig,
    const int d,
    const std::vector<DepthMat>& depths) {
  // go through source cameras and see whether any disagree
  for (int s = 0; s < int(rig.size()); ++s) {
    if (s == d) {
      continue; // don't check dst against itself
    }

    // calculate pixel in src camera that sees world point
    const Camera& src = rig[s];
    Camera::Vector2 pixel;
    if (!src.sees(world, pixel)) {
      continue;
    }

    // calculate depth for that pixel in src camera
    const DepthMat& depth = depths[s];
    CHECK_EQ(src.resolution.x(), depth.cols);
    CHECK_EQ(src.resolution.y(), depth.rows);
    const float srcDepth = depth(pixel.y(), pixel.x()); // nearest

    // calculate distance from src camera to world point
    const float proposal = (world - src.position).norm();
    if (proposal < srcDepth * FLAGS_agree_fraction) {
      return true; // proposal is closer than src, reject
    }
  }
  return false;
}

DepthMat cleanDepth(const Camera::Rig& rig, const int d, const std::vector<DepthMat>& depths) {
  const Camera& dst = rig[d];
  LOG(INFO) << boost::format("cleaning %1%") % dst.id;

  // NaN out depths that other cameras disagree with
  DepthMat depth = depths[d].clone();
  CHECK_EQ(dst.resolution.x(), depth.cols);
  CHECK_EQ(dst.resolution.y(), depth.rows);
  for (int y = 0; y < depth.rows; ++y) {
    for (int x = 0; x < depth.cols; ++x) {
      Camera::Vector3 world = dst.rig({x + 0.5, y + 0.5}, depth(y, x));
      if (isPointBad(world, rig, d, depths)) {
        depth(y, x) = NAN;
      }
    }
  }
  return depth;
}

// copy every non-nan depth from cleanDepth into depth
void restoreCleanDepth(DepthMat& depth, const DepthMat& cleanDepth) {
  for (int y = 0; y < depth.rows; ++y) {
    for (int x = 0; x < depth.cols; ++x) {
      float value = cleanDepth(y, x);
      if (!std::isnan(value)) {
        depth(y, x) = value;
      }
    }
  }
}

Camera::Rig downscale(const Camera::Rig& rig) {
  Camera::Rig result;
  for (const Camera& camera : rig) {
    Camera::Vector2 resolution = camera.resolution / FLAGS_downscale;
    result.push_back(camera.rescale(resolution.array().round()));
  }
  return result;
}

void processFrame(const std::string& frameName, const Camera::Rig& rig) {
  const filesystem::path path = filesystem::path(FLAGS_output) / frameName;
  filesystem::create_directory(path);

  // load images
  const std::vector<Image> images = image_util::loadImages<Pixel>(FLAGS_color, rig, frameName);
  const std::vector<GLuint> imageTextures =
      createTextures(images, GL_RGBA16, GL_RGBA, GL_UNSIGNED_SHORT);

  // compute initial depth estimate
  Camera::Rig small = downscale(rig);
  std::vector<DepthMat> depths(small.size());
  for (int d = 0; d < int(small.size()); ++d) {
    depths[d] = computeDepth(small, d, images, imageTextures);
    dump(path / (boost::format("%1%_iffy") % small[d].id).str(), depths[d]);
  }

  // refine depth estimate
  for (int pass = 0; pass < FLAGS_pass_count; ++pass) {
    // compute clean depths by getting rid of improbable ones
    std::vector<DepthMat> cleanDepths;
    for (int d = 0; d < int(small.size()); ++d) {
      cleanDepths.push_back(cleanDepth(small, d, depths));
      dump(path / (boost::format("%1%_%2%_clean") % small[d].id % pass).str(), cleanDepths[d]);
    }
    const std::vector<GLuint> cleanDepthTextures =
        createTextures(cleanDepths, GL_R32F, GL_RED, GL_FLOAT);

    // recompute depth using cleaned depths
    for (int d = 0; d < int(small.size()); ++d) {
      depths[d] = computeDepth(small, d, images, imageTextures, cleanDepths, cleanDepthTextures);
      dump(path / (boost::format("%1%_%2%") % small[d].id % pass).str(), depths[d]);
    }

    // restore clean depths
    if (FLAGS_keep_clean) {
      for (int d = 0; d < int(small.size()); ++d) {
        restoreCleanDepth(depths[d], cleanDepths[d]);
      }
    }

    // free clean depth textures
    for (const GLuint& texture : cleanDepthTextures) {
      glDeleteTextures(1, &texture);
    }
  }

  // free image textures
  for (const GLuint& texture : imageTextures) {
    glDeleteTextures(1, &texture);
  }
}

class OffscreenWindow : public GlWindow {
 protected:
  Camera::Rig& rig;

 public:
  OffscreenWindow(Camera::Rig& rig) : GlWindow::GlWindow(), rig(rig) {
    filesystem::create_directory(FLAGS_output);
  }

  void display() override {
    const int numFrames = std::stoi(FLAGS_last) - std::stoi(FLAGS_first) + 1;
    for (int iFrame = 0; iFrame < numFrames; ++iFrame) {
      const std::string frameName =
          image_util::intToStringZeroPad(iFrame + std::stoi(FLAGS_first), 6);
      LOG(INFO) << boost::format("Processing frame %1%") % frameName;

      processFrame(frameName, rig);
    }
  }
};

int main(int argc, char* argv[]) {
  system_util::initDep(argc, argv, kUsageMessage);

  CHECK_NE(FLAGS_rig, "");
  CHECK_NE(FLAGS_color, "");
  CHECK_NE(FLAGS_output, "");

  // prepare for offscreen rendering
  Camera::Rig rig = Camera::loadRig(FLAGS_rig);
  OffscreenWindow offscreenWindow(rig);

  // Render frames
  offscreenWindow.display();

  return EXIT_SUCCESS;
}


//Author: Radu-Tudor Andra
#include <boost/circular_buffer.hpp>
#include <pipes.hpp>
#include <vector>
#include <stdio.h>
#include <unistd.h>
#include <ctime>
#include <climits>
#include <cfloat>
#include <numeric>
#include <fcntl.h>
#include <future>
#include"engine_sensors_message.hpp"
#include"tpm_module_message.hpp"
#include"intake_sensors_message.hpp"
#include"imu_sensor_message.hpp"
#include"abs_module_message.hpp"
#include"configurable_modes_message.hpp"
#include"accelerometer_message.hpp"
#include"profiling_message.hpp"

#ifndef data_process_module_H
#define data_process_module_H

extern const int AVERAGE_OF_BUFFER;
extern const int MAX_OF_BUFFER;
extern const int MIN_OF_BUFFER;
extern const int SUM_OF_BUFFER;
extern const int FULL_BUFFER;
extern const int DO_NOT_COMPUTE;

//this is the dataProcessing class that comprises the functionality of the data processing module
class dataProcessing
{
    public:
        dataProcessing(std::vector<Pipes>,std::vector<Pipes>, std::vector<int>, int, int, int);
        void startProcessing(std::shared_future<void>);
    private:
        void readCanPipes();
        void pushBackSignals(time_t);
        void pushBackToBuckets();
        void clearBuckets();

        float compute_float_buffer_output(boost::circular_buffer<float>,int);
        bool compute_boolean_buffer_output(boost::circular_buffer<bool>,int);
        int compute_int_buffer_output(boost::circular_buffer<int>,int);
        time_t compute_time_buffer_output(boost::circular_buffer<time_t>,int);

        void sendBooleanData(boost::circular_buffer<bool>,int);
        void sendIntegerData(boost::circular_buffer<int>,int);
        void sendFloatData(boost::circular_buffer<float>,int);
        void sendTimeData(boost::circular_buffer<time_t>,int);

        time_t last_time;
        time_t last_tick_time;
        std::vector<Pipes> can_pipes_vector;
        std::vector<Pipes> output_pipe_vector;
        std::vector<int> data_mode_vector;
        int time_interval;
        int tick_interval;

        std::vector<int> processing_inputs = {AIR_TEMPERATURE, THROTTLE_POSITION,
                                              TYRE_PRESSURE_FRONT, TYRE_PRESSURE_REAR, MOTORCYCLE_SPEED,
                                              REAR_WHEEL_SPEED, FRONT_WHEEL_SPEED, BRAKE_REAR_ACTIVE,
                                              BRAKE_FRONT_ACTIVE, ABS_MODE, TC_MODE,
                                              THROTTLE_RESPONSE_MODE, LEAN_ANGLE, BATTERY_VOLTAGE,
                                              OIL_PRESSURE, GEAR_POSITION, WATER_TEMPERATURE,
                                              ENGINE_SPEED, ACCELERATION_X, ACCELERATION_Y,
                                              ACCELERATION_Z, CPU_USAGE, CPU_FREQUENCY,
                                              CPU_TEMPERATURE, MEMORY_USAGE};

        std::vector<int> processing_inputs_type = {FLOAT_TYPE, INTEGER_TYPE,
                                                   FLOAT_TYPE, FLOAT_TYPE, INTEGER_TYPE,
                                                   INTEGER_TYPE, INTEGER_TYPE, INTEGER_TYPE,
                                                   INTEGER_TYPE, INTEGER_TYPE, INTEGER_TYPE,
                                                   INTEGER_TYPE, FLOAT_TYPE, FLOAT_TYPE,
                                                   FLOAT_TYPE, INTEGER_TYPE, FLOAT_TYPE,
                                                   INTEGER_TYPE, FLOAT_TYPE, FLOAT_TYPE,
                                                   FLOAT_TYPE, FLOAT_TYPE, INTEGER_TYPE,
                                                   INTEGER_TYPE, INTEGER_TYPE};

        std::map<int,std::vector<float>>  float_buckets;
        std::map<int,std::vector<int>>    integer_buckets;
        std::map<int,std::vector<bool>> boolean_buckets;

        std::map<int,boost::circular_buffer<float>>  float_buffers;
        std::map<int,boost::circular_buffer<int>>    integer_buffers;
        std::map<int,boost::circular_buffer<bool>>   boolean_buffers;

        boost::circular_buffer<time_t>  time_stamp_buffer;

        EngineSensorsMessage    received_engine_message;
        IntakeSensorsMessage    received_intake_message;
        TPMModuleMessage        received_tpm_message;
        ABSModuleMessage        received_abs_message;
        ConfigurableModesMessage received_config_message;
        IMUSensorMessage        received_imu_message;
        AccelerometerMessage    received_accel_message;
        ProfilingMessage        received_profiling_message;

        bool new_engine_message;
        bool new_intake_message;
        bool new_tpm_message;
        bool new_abs_message;
        bool new_config_message;
        bool new_imu_message;
        bool new_accel_message;
        bool new_profiling_message;
};

#endif


#ifndef STAN_MATH_REV_MAT_FUN_SCALE_MATRIX_EXP_MULTIPLY_HPP
#define STAN_MATH_REV_MAT_FUN_SCALE_MATRIX_EXP_MULTIPLY_HPP

#include <stan/math/prim/mat.hpp>
#include <stan/math/rev/mat/fun/matrix_exp_multiply.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <vector>

namespace stan {
namespace math {

/**
 * Wrapper of matrix_exp_action function for a more literal name.
 * @tparam Ta scalar type matrix A
 * @tparam Tb scalar type matrix B
 * @tparam Cb Columns matrix B
 * @param[in] A Matrix
 * @param[in] B Matrix
 * @param[in] t double
 * @return exponential of At multiplies B
 */
template <typename Ta, typename Tb, int Cb>
inline Eigen::Matrix<typename stan::return_type<Ta, Tb>::type, -1, Cb>
scale_matrix_exp_multiply(const double& t, const Eigen::Matrix<Ta, -1, -1>& A,
                          const Eigen::Matrix<Tb, -1, Cb>& B) {
  check_nonzero_size("scale_matrix_exp_multiply", "input matrix", A);
  check_nonzero_size("scale_matrix_exp_multiply", "input matrix", B);
  check_multiplicable("scale_matrix_exp_multiply", "A", A, "B", B);
  check_square("scale_matrix_exp_multiply", "input matrix", A);
  return matrix_exp_action(A, B, t);
}

}  // namespace math
}  // namespace stan

#endif


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

#ifndef MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_
#define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_

#include <Eigen/Eigen>
#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <vector>

namespace map_based_prediction
{
using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::PredictedObject;
using autoware_auto_perception_msgs::msg::PredictedObjectKinematics;
using autoware_auto_perception_msgs::msg::PredictedObjects;
using autoware_auto_perception_msgs::msg::PredictedPath;
using autoware_auto_perception_msgs::msg::TrackedObject;
using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
using autoware_auto_perception_msgs::msg::TrackedObjects;

struct FrenetPoint
{
  double s;
  double d;
  float s_vel;
  float d_vel;
  float s_acc;
  float d_acc;
};

using FrenetPath = std::vector<FrenetPoint>;
using PosePath = std::vector<geometry_msgs::msg::Pose>;

class PathGenerator
{
public:
  PathGenerator(const double time_horizon, const double sampling_time_interval);

  PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);

  PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const;

  PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object);

  PredictedPath generatePathForOnLaneVehicle(
    const TrackedObject & object, const PosePath & ref_paths);

private:
  // Parameters
  double time_horizon_;
  double sampling_time_interval_;

  // Member functions
  PredictedPath generateStraightPath(const TrackedObject & object) const;

  PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path);

  FrenetPath generateFrenetPath(
    const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length);
  Eigen::Vector3d calcLatCoefficients(
    const FrenetPoint & current_point, const FrenetPoint & target_point, const double T);
  Eigen::Vector2d calcLonCoefficients(
    const FrenetPoint & current_point, const FrenetPoint & target_point, const double T);

  PosePath interpolateReferencePath(
    const PosePath & base_path, const FrenetPath & frenet_predicted_path);

  PredictedPath convertToPredictedPath(
    const TrackedObject & object, const FrenetPath & frenet_predicted_path,
    const PosePath & ref_path);

  FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path);
};
}  // namespace map_based_prediction

#endif  // MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_


#include "stdafx.h"

#include "PlayerHistogram.h"
#include "HistogramUtils.h"

#include "styles/ThemeParameters.h"
#include "utils/utils.h"
#include "utils/PainterPath.h"

#include "media/ptt/AudioUtils.h"

#include <boost/range/adaptor/reversed.hpp>

namespace
{
    void sampling(const std::vector<double>& a, std::vector<double>& res, size_t newSize)
    {
        if (a.empty())
        {
            res.clear();
            return;
        }
        res.resize(newSize);

        for (size_t i = 0; i < newSize - 1; ++i) {
            int index = i * (a.size() - 1) / (newSize - 1);
            int p = i * (a.size() - 1) % (newSize - 1);

            res[i] = ((p * a[index + 1]) + (((newSize - 1) - p) * a[index])) / (newSize - 1);
        }
        res[newSize - 1] = a[a.size() - 1];
    }
}

namespace Ui
{
    PlayerHistogram::PlayerHistogram(QWidget* _parent)
        : ClickableWidget(_parent)
    {
        setAttribute(Qt::WA_TransparentForMouseEvents, true);
    }

    PlayerHistogram::~PlayerHistogram() = default;

    void PlayerHistogram::start()
    {
        isActive_ = true;
        clear();
        update();
    }

    void PlayerHistogram::stop()
    {
        isActive_ = false;
        resample(width());
        update();
    }

    void PlayerHistogram::clear()
    {
        ampl_.clear();
        resampledAmpl_.clear();
        currentSample_ = -1;
    }

    bool PlayerHistogram::isActive() const
    {
        return isActive_;
    }

    bool PlayerHistogram::hasSamples() const
    {
        return !ampl_.empty();
    }

    void PlayerHistogram::addSpectrum(const QVector<double>& _v)
    {
        for (auto x : _v)
            ampl_.push_back(x);

        update();
    }

    void PlayerHistogram::setCurrentSample(int _current)
    {
        if (currentSample_ != _current)
        {
            currentSample_ = _current;
            update();
        }
    }

    std::optional<PlayerHistogram::ScaledCoeff> PlayerHistogram::sampleUnderCursor() const
    {
        if (resampledAmpl_.empty())
            return {};
        const auto pos = mapFromGlobal(QCursor::pos());
        if (pos.x() < 0 || pos.x() > width())
            return {};

        const int idx = std::clamp(pos.x() / (histogram::getSampleWidth() + histogram::getSampleSpace()), 0, int(resampledAmpl_.size() - 1));
        return ScaledCoeff{ idx, double(resampledAmpl_.size()) / ampl_.size() };
    }

    void PlayerHistogram::paintEvent(QPaintEvent*)
    {
        QPainter p(this);
        p.setRenderHint(QPainter::Antialiasing);
        p.setRenderHint(QPainter::SmoothPixmapTransform);

        const auto h = height();
        const auto w = width();

        static const auto sampleWidth = histogram::getSampleWidth();
        static const auto space = histogram::getSampleSpace();

        const double volumeFactor = histogram::getVolumeFactor(volume_);

        enum class Direction
        {
            direct,
            reverse
        };

        auto fillByDots = [sampleWidth = sampleWidth, space = space, w, h, &p](auto& currentPos, const QColor& color, Direction direction)
        {
            if (direction == Direction::reverse)
            {
                while (currentPos >= 0)
                {
                    p.fillPath(histogram::makeSamplePath(0, 1.0, currentPos, h), color);
                    currentPos -= sampleWidth + space;
                }
            }
            else
            {
                while (currentPos < w)
                {
                    p.fillPath(histogram::makeSamplePath(0, 1.0, currentPos, h), color);
                    currentPos += sampleWidth + space;
                }
            }
        };

        int currentPos = 0;

        if (isActive())
        {
            currentPos = w;
            const auto color = Styling::getParameters().getColor(Styling::StyleVariable::BASE_GLOBALWHITE);
            if (int(resampledAmpl_.size() * (sampleWidth + space)) > w)
            {
                for (auto x : boost::adaptors::reverse(resampledAmpl_))
                {
                    p.fillPath(histogram::makeSamplePath(x, volumeFactor, currentPos, h), color);
                    currentPos -= sampleWidth + space;

                    if (currentPos <= 0)
                        break;
                }
            }
            else
            {
                for (auto x : boost::adaptors::reverse(resampledAmpl_))
                {
                    p.fillPath(histogram::makeSamplePath(x, volumeFactor, currentPos, h), color);
                    currentPos -= sampleWidth + space;

                    if (currentPos <= 0)
                        break;
                }

                fillByDots(currentPos, color, Direction::reverse);
            }
        }
        else
        {
            const auto color1 = Styling::getParameters().getColor(Styling::StyleVariable::PRIMARY);
            const auto color2 = Styling::getParameters().getColor(Styling::StyleVariable::BASE_BRIGHT_HOVER);
            const int resmapledCurrentSample = (currentSample_ > 0  && currentSample_ != std::numeric_limits<int>::max()) ? int(currentSample_ * (double(resampledAmpl_.size()) / ampl_.size()) + 0.5) : currentSample_;
            int current = 0;
            for (auto x : resampledAmpl_)
            {
                p.fillPath(histogram::makeSamplePath(x, volumeFactor, currentPos, h), current <= resmapledCurrentSample ? color1 : color2);
                currentPos += sampleWidth + space;
                ++current;
            }
            fillByDots(currentPos, resmapledCurrentSample >= int(resampledAmpl_.size()) ? color1 : color2, Direction::direct);
        }
    }

    void PlayerHistogram::resizeEvent(QResizeEvent* _e)
    {
        ClickableWidget::resizeEvent(_e);
        if (const auto newWidgth = _e->size().width(); newWidgth != _e->oldSize().width())
            resample(newWidgth);
    }

    void PlayerHistogram::resample(int _width)
    {
        static const auto sampleWidth = histogram::getSampleWidth();
        static const auto space = histogram::getSampleSpace();

        const auto neededSize = size_t(double(_width) / (sampleWidth + space) + 0.5);

        sampling(ampl_, resampledAmpl_, neededSize);
    }

    int PlayerHistogram::getSampleWidth()
    {
        return histogram::getSampleWidth();
    }

    void PlayerHistogram::setVolume(double _v)
    {
        volume_ = _v;
    }
}