// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

/// @file
/// Definitions corresponding to vc.hpp.

#include <Eigen/Dense>
#include <exceptions.hpp>
#include <periodic_table.hpp>
#include <vc.hpp>

namespace alma {
std::unique_ptr<Crystal_structure> vc_mix_structures(
    const std::vector<Crystal_structure>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Perform some sanity checks. Note that even passing all of
    // those does not guarantee that the resulting virtual crystal
    // will make sense.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    auto natoms = components[0].get_natoms();

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        if (components[i].get_natoms() != natoms)
            throw value_error("all components must have the same"
                              " number of atoms");
    }
    // Build the weighted lattice vectors.
    double total{ratios[0]};
    Eigen::Matrix3d lattvec(ratios[0] * components[0].lattvec);

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        total += ratios[i];
        lattvec += ratios[i] * components[i].lattvec;
    }
    lattvec /= total;
    // Build the weighted atomic positions.
    Eigen::Matrix<double, 3, Eigen::Dynamic> positions(ratios[0] *
                                                       components[0].positions);

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i)
        positions += ratios[i] * components[i].positions;
    positions /= total;
    // And the set of virtual elements.
    std::vector<std::string> vnames;
    std::vector<int> numbers;

    for (decltype(natoms) ia = 0; ia < natoms; ++ia) {
        std::vector<std::string> symbols;

        for (auto& c : components)
            symbols.emplace_back(c.get_element(ia));
        // Adjacent atoms belonging to the same element are grouped
        // together.
        auto name = Virtual_element(symbols, ratios).get_name();

        if ((vnames.size() == 0) || (vnames.back() != name)) {
            vnames.emplace_back(name);
            numbers.emplace_back(1);
        }
        else
            ++numbers.back();
    }
    return std::make_unique<Crystal_structure>(
        lattvec, positions, vnames, numbers);
}

std::unique_ptr<Dielectric_parameters> vc_mix_dielectric_parameters(
    const std::vector<Dielectric_parameters>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Basic sanity checks.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    auto nborn = components[0].born.size();
    double total{ratios[0]};
    Eigen::Matrix3d epsilon{ratios[0] * components[0].epsilon};
    std::vector<Eigen::MatrixXd> born;

    for (decltype(nborn) ib = 0; ib < nborn; ++ib)
        born.emplace_back(Eigen::MatrixXd{ratios[0] * components[0].born[ib]});

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        if (components[i].born.size() != nborn)
            throw value_error("all components must contain the same"
                              " number of Born charge tensors");
        total += ratios[i];
        epsilon += ratios[i] * components[i].epsilon;

        for (decltype(nborn) ib = 0; ib < nborn; ++ib)
            born[ib] += ratios[i] * components[i].born[ib];
    }
    epsilon /= total;

    for (auto& b : born)
        b /= total;
    return std::make_unique<Dielectric_parameters>(born, epsilon);
}

std::unique_ptr<Harmonic_ifcs> vc_mix_harmonic_ifcs(
    const std::vector<Harmonic_ifcs>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Basic sanity checks.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    // Only the ifcs themselves are averaged. The remainin
    // attributes must be common to all objects.
    double total{ratios[0]};
    auto na = components[0].na;
    auto nb = components[0].nb;
    auto nc = components[0].nc;
    auto nblocks = components[0].ifcs.size();
    std::vector<Triple_int> pos{components[0].pos};
    std::vector<Eigen::MatrixXd> blocks;

    for (decltype(nblocks) ib = 0; ib < nblocks; ++ib)
        blocks.emplace_back(
            Eigen::MatrixXd{ratios[0] * components[0].ifcs[ib]});

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i) {
        if (components[i].ifcs.size() != nblocks)
            throw value_error("all components must contain the same"
                              " number of IFC matrices");
        total += ratios[i];

        for (decltype(nblocks) ib = 0; ib < nblocks; ++ib) {
            if (components[i].pos[ib] != pos[ib])
                throw value_error("all components must contain IFC matrices"
                                  " for the same cell pairs");
            blocks[ib] += ratios[i] * components[i].ifcs[ib];
        }
    }

    for (auto& b : blocks)
        b /= total;
    return std::make_unique<Harmonic_ifcs>(pos, blocks, na, nb, nc);
}

std::unique_ptr<std::vector<Thirdorder_ifcs>> vc_mix_thirdorder_ifcs(
    const std::vector<std::vector<Thirdorder_ifcs>>& components,
    const std::vector<double>& ratios) {
    auto ncomponents = components.size();

    // Basic sanity checks.
    if (ratios.size() != ncomponents)
        throw value_error("different numbers of components and ratios");

    if (ncomponents == 0)
        throw value_error("no components in the virtual crystal");

    for (auto r : ratios)
        if (r <= 0)
            throw value_error("all atomic ratios must be positive");
    auto nifcs = components[0].size();

    for (decltype(ncomponents) i = 1; i < ncomponents; ++i)
        if (components[i].size() != nifcs)
            throw value_error("all components must contain the same number"
                              " of IFC blocks");
    // Create the final object.
    auto nruter = std::make_unique<std::vector<Thirdorder_ifcs>>();
    // We average the Cartesian coordinates of the unit cells and
    // the IFCs. We require the atom indices to be common to all
    // inputs.
    double total = std::accumulate(ratios.begin(), ratios.end(), 0.);

    for (decltype(nifcs) ic = 0; ic < nifcs; ++ic) {
        auto i = components[0][ic].i;
        auto j = components[0][ic].j;
        auto k = components[0][ic].k;
        Eigen::VectorXd rj{ratios[0] * components[0][ic].rj};
        Eigen::VectorXd rk{ratios[0] * components[0][ic].rk};

        for (decltype(ncomponents) ir = 1; ir < ncomponents; ++ir) {
            if ((components[ir][ic].i != i) || (components[ir][ic].j != j) ||
                (components[ir][ic].k != k))
                throw value_error("all components must contain IFC blocks"
                                  " for the same atom triplets");
            rj += ratios[ir] * components[ir][ic].rj;
            rk += ratios[ir] * components[ir][ic].rk;
        }
        rj /= total;
        rk /= total;
        Thirdorder_ifcs block{rj, rk, i, j, k};

        for (std::size_t alpha = 0; alpha < 3; ++alpha)
            for (std::size_t beta = 0; beta < 3; ++beta)
                for (std::size_t gamma = 0; gamma < 3; ++gamma) {
                    block.ifc(alpha, beta, gamma) =
                        ratios[0] * components[0][ic].ifc(alpha, beta, gamma);

                    for (decltype(ncomponents) ir = 1; ir < ncomponents; ++ir)
                        block.ifc(alpha, beta, gamma) +=
                            ratios[ir] *
                            components[ir][ic].ifc(alpha, beta, gamma);
                    block.ifc(alpha, beta, gamma) /= total;
                }
        nruter->emplace_back(block);
    }
    return nruter;
}
} // namespace alma


#include "darknet.h"
#include "yolo.h"

#include <dlib/data_io.h>
#include <iostream>

#undef VISUAL

#include <dlib/gui_widgets.h>

struct training_sample
{
    dlib::matrix<dlib::rgb_pixel> image;
    std::vector<dlib::mmod_rect> boxes;
    dlib::yolo_options::map_type yolo_map;
};

auto main(const int argc, const char** argv) -> int
try
{
    if (argc > 1)
    {
        dlib::image_window win;
        darknet::detector19_infer net;
        {
            darknet::detector19_infer temp;
            auto trainer = dlib::dnn_trainer(temp);
            trainer.set_synchronization_file("yolo_darknet19_sync");
            net = trainer.get_net(dlib::force_flush_to_disk::no);
        }

        std::cout << net << '\n';
        dlib::matrix<dlib::rgb_pixel> image, input_image;
        for (int i = 1; i < argc; ++i)
        {
            dlib::load_image(image, argv[i]);
            dlib::letter_box(
                image,
                net.loss_details().get_options().get_input_size(),
                input_image);
            win.set_image(input_image);
            const auto detections = net.process(input_image, -1);
            std::cout << "found " << detections.size() << " detections\n";
            for (const auto& det : detections)
            {
                std::cout << det.label << ", " << det.detection_confidence << ", " << det.rect
                          << '\n';
                win.add_overlay(
                    det.rect,
                    dlib::rgb_pixel(255, 0, 0),
                    det.label + ": " + std::to_string(det.detection_confidence));
            }
            std::cin.get();
            win.clear_overlay();
        }
    }
    else
    {
        std::vector<dlib::matrix<dlib::rgb_pixel>> images;
        std::vector<std::vector<dlib::mmod_rect>> bboxes;
        dlib::load_image_dataset(images, bboxes, "./pascal.xml");
        std::cout << "image dataset loaded: " << images.size() << " images\n";

        const long input_size = 448;
        dlib::yolo_options options(input_size, 32, bboxes);
        darknet::detector19_train net(options);
        dlib::set_all_bn_inputs_no_bias(net);
        dlib::visit_computational_layers(net, [](dlib::leaky_relu_& l) {
            l = dlib::leaky_relu_(0.2);
        });

        net.subnet().layer_details().set_num_filters(options.get_labels().size() + 5);
        {
            dlib::matrix<dlib::rgb_pixel> dummy(input_size, input_size);
            net(dummy);
        }
        std::cout << net << '\n';
        std::cout << "num parameters: " << dlib::count_parameters(net) << '\n';
        std::cout << "num convolutions: " << count_convolutions(net) << '\n';
        dlib::pipe<training_sample> training_data(1000);

#ifdef VISUAL
        dlib::image_window win;
        auto data_loader = [&win, &training_data, &images, &bboxes, &options](time_t seed)
#else
        auto data_loader = [&training_data, &images, &bboxes, &options](time_t seed)
#endif
        {
            dlib::rand rnd(time(nullptr) + seed);

            // const long min_size = options.get_downsampling_factor() * 1.5;
            // const long chip_dim = options.get_input_size();
            // dlib::random_cropper cropper;
            // cropper.set_seed(time(nullptr) + seed);
            // cropper.set_chip_dims(chip_dim, chip_dim);
            // cropper.set_translate_amount(0.1);
            // cropper.set_background_crops_fraction(0);
            // cropper.set_randomly_flip(true);
            // cropper.set_max_rotation_degrees(0);
            // cropper.set_min_object_size(min_size, min_size);
            // cropper.set_max_object_size(0.9);

            training_sample sample;
            while (training_data.is_enabled())
            {
                const auto idx = rnd.get_random_32bit_number() % images.size();
                // const auto& image = images[idx];
                // const auto& boxes = bboxes[idx];
                // cropper(image, boxes, sample.image, sample.boxes);

                const auto temp = options.generate_map(images[idx], bboxes[idx]);
                sample.image = std::move(temp.first);
                sample.yolo_map = std::move(temp.second);
                dlib::disturb_colors(sample.image, rnd);

#ifdef VISUAL
                dlib::matrix<dlib::rgb_pixel> square_image;
                const auto [scale, offset] =
                    dlib::letter_box(images[idx], options.get_input_size(), square_image);
                win.set_image(sample.image);
                for (const auto& box : bboxes[idx])
                {
                    sample.boxes.push_back(
                        dlib::translate_rect(dlib::scale_rect(box.rect, scale), offset));
                    win.add_overlay(sample.boxes.back(), dlib::rgb_pixel(0, 255, 0), box.label);
                }
                std::cin.get();
                win.clear_overlay();
                win.set_image(options.overlay_map(images[idx], bboxes[idx]));
                std::cin.get();
#endif

                training_data.enqueue(sample);
            }
        };

        std::vector<std::thread> data_loaders;
        for (int i = 0; i < 1; ++i)
        {
            data_loaders.emplace_back([&data_loader, i]() { data_loader(i + 1); });
        }

        auto trainer = dlib::dnn_trainer(net);
        trainer.be_verbose();
        trainer.set_learning_rate(0.1);
        trainer.set_iterations_without_progress_threshold(3000);
        trainer.set_mini_batch_size(32);
        trainer.set_synchronization_file("yolo_darknet19_sync", std::chrono::minutes(10));
        std::cout << trainer << '\n';
        std::vector<dlib::matrix<dlib::rgb_pixel>> minibatch_images;
        std::vector<dlib::yolo_options::map_type> minibatch_labels;
        while (trainer.get_learning_rate() > 1e-5)
        {
            minibatch_images.clear();
            minibatch_labels.clear();
            training_sample sample;
            while (minibatch_images.size() < trainer.get_mini_batch_size())
            {
                training_data.dequeue(sample);
                minibatch_images.push_back(sample.image);
                minibatch_labels.push_back(sample.yolo_map);
            }
            trainer.train_one_step(minibatch_images, minibatch_labels);
        }

        training_data.disable();
        for (auto& dl : data_loaders)
        {
            dl.join();
        }
        trainer.get_net();
        net.clean();
        dlib::serialize("yolo-darknet19.dnn") << net;
    }
}
catch (const std::exception& e)
{
    std::cout << e.what() << '\n';
}


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "dart/constraint/PgsBoxedLcpSolver.hpp"

#include <cmath>
#include <cstring>
#include <Eigen/Dense>
#include "dart/external/odelcpsolver/matrix.h"
#include "dart/external/odelcpsolver/misc.h"
#include "dart/math/Constants.hpp"

#define PGS_EPSILON 10e-9

namespace dart {
namespace constraint {

//==============================================================================
PgsBoxedLcpSolver::Option::Option(
    int maxIteration,
    double deltaXTolerance,
    double relativeDeltaXTolerance,
    double epsilonForDivision,
    bool randomizeConstraintOrder)
  : mMaxIteration(maxIteration),
    mDeltaXThreshold(deltaXTolerance),
    mRelativeDeltaXTolerance(relativeDeltaXTolerance),
    mEpsilonForDivision(epsilonForDivision),
    mRandomizeConstraintOrder(randomizeConstraintOrder)
{
  // Do nothing
}

//==============================================================================
const std::string& PgsBoxedLcpSolver::getType() const
{
  return getStaticType();
}

//==============================================================================
const std::string& PgsBoxedLcpSolver::getStaticType()
{
  static const std::string type = "PgsBoxedLcpSolver";
  return type;
}

//==============================================================================
bool PgsBoxedLcpSolver::solve(
    int n,
    double* A,
    double* x,
    double* b,
    int nub,
    double* lo,
    double* hi,
    int* findex,
    bool /*earlyTermination*/)
{
  const int nskip = dPAD(n);

  // If all the variables are unbounded then we can just factor, solve, and
  // return.R
  if (nub >= n)
  {
    mCacheD.resize(n);
    std::fill(mCacheD.begin(), mCacheD.end(), 0);

    dFactorLDLT(A, mCacheD.data(), n, nskip);
    dSolveLDLT(A, mCacheD.data(), b, n, nskip);
    std::memcpy(x, b, n * sizeof(double));

    return true;
  }

  mCacheOrder.clear();
  mCacheOrder.reserve(n);

  bool possibleToTerminate = true;
  for (int i = 0; i < n; ++i)
  {
    // mOrderCacheing
    if (A[nskip * i + i] < mOption.mEpsilonForDivision)
    {
      x[i] = 0.0;
      continue;
    }

    mCacheOrder.push_back(i);

    // Initial loop
    const double* A_ptr = A + nskip * i;
    const double old_x = x[i];

    double new_x = b[i];

    for (int j = 0; j < i; ++j)
      new_x -= A_ptr[j] * x[j];

    for (int j = i + 1; j < n; ++j)
      new_x -= A_ptr[j] * x[j];

    new_x /= A[nskip * i + i];

    if (findex[i] >= 0)
    {
      const double hi_tmp = hi[i] * x[findex[i]];
      const double lo_tmp = -hi_tmp;

      if (new_x > hi_tmp)
        x[i] = hi_tmp;
      else if (new_x < lo_tmp)
        x[i] = lo_tmp;
      else
        x[i] = new_x;
    }
    else
    {
      if (new_x > hi[i])
        x[i] = hi[i];
      else if (new_x < lo[i])
        x[i] = lo[i];
      else
        x[i] = new_x;
    }

    // Test
    if (possibleToTerminate)
    {
      const double deltaX = std::abs(x[i] - old_x);
      if (deltaX > mOption.mDeltaXThreshold)
        possibleToTerminate = false;
    }
  }

  if (possibleToTerminate)
  {
    return true;
  }

  // Normalizing
  for (const auto& index : mCacheOrder)
  {
    const double dummy = 1.0 / A[nskip * index + index];
    b[index] *= dummy;
    for (int j = 0; j < n; ++j)
      A[nskip * index + j] *= dummy;
  }

  for (int iter = 1; iter < mOption.mMaxIteration; ++iter)
  {
    if (mOption.mRandomizeConstraintOrder)
    {
      if ((iter & 7) == 0)
      {
        for (std::size_t i = 1; i < mCacheOrder.size(); ++i)
        {
          const int tmp = mCacheOrder[i];
          const int swapi = dRandInt(i + 1);
          mCacheOrder[i] = mCacheOrder[swapi];
          mCacheOrder[swapi] = tmp;
        }
      }
    }

    possibleToTerminate = true;

    // Single loop
    for (const auto& index : mCacheOrder)
    {
      const double* A_ptr = A + nskip * index;
      double new_x = b[index];
      const double old_x = x[index];

      for (int j = 0; j < index; j++)
        new_x -= A_ptr[j] * x[j];

      for (int j = index + 1; j < n; j++)
        new_x -= A_ptr[j] * x[j];

      if (findex[index] >= 0)
      {
        const double hi_tmp = hi[index] * x[findex[index]];
        const double lo_tmp = -hi_tmp;

        if (new_x > hi_tmp)
          x[index] = hi_tmp;
        else if (new_x < lo_tmp)
          x[index] = lo_tmp;
        else
          x[index] = new_x;
      }
      else
      {
        if (new_x > hi[index])
          x[index] = hi[index];
        else if (new_x < lo[index])
          x[index] = lo[index];
        else
          x[index] = new_x;
      }

      if (possibleToTerminate
          && std::abs(x[index]) > mOption.mEpsilonForDivision)
      {
        const double relativeDeltaX = std::abs((x[index] - old_x) / x[index]);
        if (relativeDeltaX > mOption.mRelativeDeltaXTolerance)
          possibleToTerminate = false;
      }
    }

    if (possibleToTerminate)
      break;
  }

  return possibleToTerminate;
}

#ifndef NDEBUG
//==============================================================================
bool PgsBoxedLcpSolver::canSolve(int n, const double* A)
{
  const int nskip = dPAD(n);

  // Return false if A has zero-diagonal or A is nonsymmetric matrix
  for (auto i = 0; i < n; ++i)
  {
    if (A[nskip * i + i] < PGS_EPSILON)
      return false;

    for (auto j = 0; j < n; ++j)
    {
      if (std::abs(A[nskip * i + j] - A[nskip * j + i]) > PGS_EPSILON)
        return false;
    }
  }

  return true;
}
#endif

//==============================================================================
void PgsBoxedLcpSolver::setOption(const PgsBoxedLcpSolver::Option& option)
{
  mOption = option;
}

//==============================================================================
const PgsBoxedLcpSolver::Option& PgsBoxedLcpSolver::getOption() const
{
  return mOption;
}

} // namespace constraint
} // namespace dart


#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include "debug.h"
#include "types.h"
#include "imgproc.h"
#include "photobundle.h"
#include "sample_eigen.h"
#include "utils.h"

#if defined(WITH_CEREAL)
#include "ceres_cereal.h"
#include "eigen_cereal.h"
#include <fstream>
#include <cereal/archives/binary.hpp>
#include <cereal/types/memory.hpp>
#endif

#include <cassert>
#include <cmath>
#include <type_traits>
#include <iterator>
#include <algorithm>
#include <map>

// this is just for YCM to stop highlighting openmp as error (there is no openmp
// in clang3.5)
#define HAS_OPENMP __GNUC__ >= 4 && __clang__ == 0

#if HAS_OPENMP
#include <omp.h>
#endif

#include <Eigen/Geometry>

static PhotometricBundleAdjustment::Options::DescriptorType
DescriptorTypeFromString(std::string s)
{
  if(utils::icompare("Intensity", s))
    return PhotometricBundleAdjustment::Options::DescriptorType::Intensity;
  else if(utils::icompare("IntensityAndGradient", s))
    return PhotometricBundleAdjustment::Options::DescriptorType::IntensityAndGradient;
  else if(utils::icompare("BitPlanes", s))
    return PhotometricBundleAdjustment::Options::DescriptorType::BitPlanes;
  else {
    Warn("Unknown descriptorType '%s'\n", s.c_str());
    return PhotometricBundleAdjustment::Options::DescriptorType::Intensity;
  }
}

PhotometricBundleAdjustment::Result
PhotometricBundleAdjustment::Result::FromFile(std::string filename)
{
#if defined(WITH_CEREAL)
  std::ifstream ifs(filename);
  if(ifs.is_open()) {
    cereal::BinaryInputArchive ar(ifs);
    Result ret;
    ar(ret);
    return ret;
  } else {
    Fatal("Failed to open %s\n", filename.c_str());
  }
#else
  UNUSED(filename);
  Fatal("compile WITH_CEREAL\n");
#endif
}

bool PhotometricBundleAdjustment::Result::Writer::add(const Result& result)
{
  bool ret = false;

#if defined(WITH_CEREAL)
  std::ofstream ofs(utils::Format("%s/%05d.out", _prefix.c_str(), _counter++));
  if(ofs.is_open()) {
    cereal::BinaryOutputArchive ar(ofs);
    ar(result);
    ret = true;
  }
#else
  utils::UNUSED(result);
#endif

  return ret;
}

PhotometricBundleAdjustment::Options::Options(const utils::ConfigFile& cf)
  : maxNumPoints(cf.get<int>("maxNumPoints", 4096)),
    slidingWindowSize(cf.get<int>("slidingWindowSize", 5)),
    patchRadius(cf.get<int>("patchRadius", 2)),
    maskBlockRadius(cf.get<int>("maskBlockRadius", 1)),
    maxFrameDistance(cf.get<int>("maxFrameDistance", 1)),
    numThreads(cf.get<int>("numThreads", -1)),
    doGaussianWeighting((bool) cf.get<int>("doGaussianWeighting", 0)),
    verbose((bool) cf.get<int>("verbose", 1)),
    minScore(cf.get<double>("minScore", 0.75)),
    robustThreshold(cf.get<double>("robustThreshold", 0.05)),
    minValidDepth(cf.get<double>("minValidDepth", 0.01)),
    maxValidDepth(cf.get<double>("maxValidDepth", 1000.0)),
    nonMaxSuppRadius(cf.get<int>("nonMaxSuppRadius", 1)),
    descriptorType(DescriptorTypeFromString(cf.get<std::string>("descriptorType", "Intensity")))
{}

/**
 * simple class to store the image gradient
 */
class ImageGradient
{
 public:
  typedef Image_<float> ImageT;

 public:
  ImageGradient() = default;
  ImageGradient(const ImageT& Ix, const ImageT& Iy)
      : _Ix(Ix), _Iy(Iy) {}

  inline const ImageT& Ix() const { return _Ix; }
  inline const ImageT& Iy() const { return _Iy; }

  inline ImageT absGradientMag() const
  {
    return _Ix.array().abs() + _Iy.array().abs();
  }

  template <class InputImage>
  inline void compute(const InputImage& I)
  {
    static_assert(std::is_same<typename InputImage::Scalar, uint8_t>::value ||
                  std::is_same<typename InputImage::Scalar, float>::value,
                  "type mismatch, input image must be uint8_t or float");

    optResize(I.rows(), I.cols());
    imgradient(I.data(), ImageSize(I.rows(), I.cols()), _Ix.data(), _Iy.data());
  }

 private:
  ImageT _Ix;
  ImageT _Iy;

  void optResize(int rows, int cols)
  {
    if(_Ix.rows() != rows || _Ix.cols() != cols) {
      _Ix.resize(rows, cols);
      _Iy.resize(rows, cols);
    }
  }
}; // ImageGradient

class PhotometricBundleAdjustment::DescriptorFrame
{
 public:
  typedef EigenAlignedContainer_<Image_<float>> Channels;
  typedef EigenAlignedContainer_<ImageGradient> ImageGradientList;

 public:
  /**
   * \param frame_id the frame number (unique per image)
   * \param I        grayscale input image
   * \param gx       list of x-gradients per channel
   * \param gy       list of y-gradients per channel
   */
  inline DescriptorFrame(uint32_t frame_id, const Channels& channels)
      : _frame_id(frame_id), _channels(channels)
  {
    assert( !_channels.empty() );

    _max_rows = _channels[0].rows() - 1;
    _max_cols = _channels[0].cols() - 1;

    _gradients.resize( _channels.size() );
    for(size_t i = 0; i < _channels.size(); ++i) {
      _gradients[i].compute(_channels[i]);
    }
  }

  DescriptorFrame(const DescriptorFrame&) = delete;
  DescriptorFrame& operator=(const DescriptorFrame&) = delete;

  inline uint32_t id() const { return _frame_id; }

  inline bool operator<(const DescriptorFrame& other) const
  {
    return _frame_id < other._frame_id;
  }

  inline size_t numChannels() const { return _channels.size(); }

  inline const Image_<float>& getChannel(size_t i) const
  {
    assert(i < _channels.size());
    return _channels[i];
  }

  inline const ImageGradient& getChannelGradient(size_t i) const
  {
    assert(i < _gradients.size());
    return _gradients[i];
  }

  /**
   * \return true of point projects to the image
   */
  template <class ProjType> inline
  bool isProjectionValid(const ProjType& x) const
  {
    return x[0] >= 0.0 && x[0] < _max_cols &&
           x[1] >= 0.0 && x[1] < _max_rows;
  }

  void computeSaliencyMap(Image_<float>& smap) const
  {
    assert( !_gradients.empty() );

    smap.array() = _gradients[0].absGradientMag();
    for(size_t i = 1; i < _gradients.size(); ++i) {
      smap.array() += _gradients[i].absGradientMag().array();
    }
  }

 public:
  static inline DescriptorFrame* Create(uint32_t id, const Image_<uint8_t>& image,
                                        PhotometricBundleAdjustment::Options::DescriptorType type)
  {
    Channels channels;
    switch(type) {
      case PhotometricBundleAdjustment::Options::DescriptorType::Intensity:
        channels.push_back( image.cast<Channels::value_type::Scalar>() );
        break;
      case PhotometricBundleAdjustment::Options::DescriptorType::IntensityAndGradient: {
        channels.push_back( image.cast<Channels::value_type::Scalar>() );
        channels.push_back( Image_<float>(image.rows(), image.cols()) );
        channels.push_back( Image_<float>(image.rows(), image.cols()) );

        imgradient(image.data(), ImageSize(image.rows(), image.cols()),
                   channels[1].data(), channels[2].data());

      } break;
      case PhotometricBundleAdjustment::Options::DescriptorType::BitPlanes: {
        computeBitPlanes(image.data(), ImageSize(image.rows(), image.cols()), channels);
      } break;
    }

    return new DescriptorFrame(id, channels);
  }

 private:
  uint32_t _frame_id;
  uint32_t _max_rows;
  uint32_t _max_cols;

  Channels _channels;
  ImageGradientList _gradients;
}; // DescriptorFrame

/**
 * \return bilinearly interpolated pixel value at subpixel location (xf,yf)
 */
template <class Image, class T> inline
T interp2(const Image& I, T xf, T yf, T fillval = 0.0, T offset = 0.0)
{
  const int max_cols = I.cols() - 1;
  const int max_rows = I.rows() - 1;

  xf += offset;
  yf += offset;

  int xi = (int) std::floor(xf);
  int yi = (int) std::floor(yf);

  xf -= xi;
  yf -= yi;

  if( xi >= 0 && xi < max_cols && yi >= 0 && yi < max_rows )
  {
    const T wx = 1.0 - xf;
    return (1.0 - yf) * ( I(yi,   xi)*wx + I(yi,   xi+1)*xf )
               +  yf  * ( I(yi+1, xi)*wx + I(yi+1, xi+1)*xf );
  } else
  {
    if( xi == max_cols && yi < max_rows )
      return ( xf > 0 ) ? fillval : (1.0-yf)*I(yi,xi) + yf*I(yi+1, xi);
    else if( yi == max_rows && xi < max_cols )
      return ( yf > 0 ) ? fillval : (1.0-xf)*I(yi,xi) + xf*I(yi, xi+1);
    else if( xi == max_cols && yi == max_rows )
      return ( xf > 0 || yf > 0 ) ? fillval : I(yi, xi);
    else
      return fillval;
  }

}

template <int N> constexpr int square() { return N*N; }

template <int R, class ImageType, class ProjType, typename T = double>
void interpolateFixedPatch(Vec_<T, square<2*R+1>()>& dst,
                           const ImageType& I, const ProjType& p,
                           const T& fillval = T(0), const T& offset = T(0))
{
  const T x = static_cast<T>( p[0] + offset );
  const T y = static_cast<T>( p[1] + offset );

  auto d_ptr = dst.data();
  for(int r = -R; r <= R; ++r) {
    for(int c = -R; c <= R; ++c) {
      *d_ptr++ = interp2(I, c + x, r + y, fillval);
    }
  }
}

template <int R, typename T = float>
class ZnccPatch_
{
  static_assert(std::is_floating_point<T>::value, "T must be floating point");

 public:
  static constexpr int Radius    = R;
  static constexpr int Dimension = (2*R+1) * (2*R+1);

 public:
  inline ZnccPatch_() {}

  template <class ImageType, class ProjType> inline
  ZnccPatch_(const ImageType& image, const ProjType& uv) { set(image, uv); }

  template <class ImageType, class ProjType> inline
  const ZnccPatch_& set(const ImageType& I, const ProjType& uv)
  {
    interpolateFixedPatch<R>(_data, I, uv, T(0.0), T(0.0));
    T mean = _data.array().sum() / (T) _data.size();
    _data.array() -= mean;
    _norm = _data.norm();

    return *this;
  }

  template <class ImageType, class ProjType>
  inline static ZnccPatch_ FromImage(const ImageType& I, const ProjType& p)
  {
    ZnccPatch_ ret;
    ret.set(I, p);
    return ret;
  }

  inline T score(const ZnccPatch_& other) const
  {
    T d = _norm * other._norm;
    return d > 1e-6 ? _data.dot(other._data) / d : -1.0;
  }

 private:
  Vec_<T, Dimension> _data;
  T _norm;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
}; // ZnccPatch

/**
 */
struct PhotometricBundleAdjustment::ScenePoint
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  typedef std::vector<uint32_t>        VisibilityList;
  typedef EigenAlignedContainer_<Vec2> ProjectionList;
  typedef ZnccPatch_<2, float>         ZnccPatchType;

  /**
   * Create a scene point with position 'X' and reference frame number 'f_id'
   *
   * We also store the original point for later comparision
   */
  inline ScenePoint(const Vec3& X, uint32_t f_id)
      : _X(X), _X_original(X)
  {
    _f.reserve(8);
    _f.push_back(f_id);
  }

  /**
   * \return true if the scene point has 'f_id' it is visibility list
   */
  inline bool hasFrame(uint32_t f_id) const {
    return std::find(_f.begin(), _f.end(), f_id) != _f.end();
  }

  /**
   * \return the visibility list
   */
  inline const VisibilityList& visibilityList() const { return _f; }

  /** \return the reference frame number (also the first in the list) */
  inline const uint32_t& refFrameId() const { return _f.front(); }

  /** \return the last frame number, most recent observation */
  inline const uint32_t& lastFrameId() const { return _f.back(); }

  /** \return the 3D point associated with the ScenePoint */
  inline const Vec3& X() const { return _X; }
  inline       Vec3& X()       { return _X; }

  /** \return the original 3D point */
  inline const Vec3& getOriginalPoint() const { return _X_original; }

  /** \return the associated patch */
  inline const ZnccPatchType& patch() const { return _patch; }

  inline void addFrame(uint32_t f) { _f.push_back(f); }

  template <class ImageType, class ProjType> inline
  void setZnccPach(const ImageType& I, const ProjType& x)
  {
    _patch.set(I, x);
  }

  inline const std::vector<double>& descriptor() const { return _descriptor; }
  inline       std::vector<double>& descriptor()       { return _descriptor; }

  inline void setSaliency(double v) { _saliency = v; }
  inline const double& getSaliency() const { return _saliency; }

  inline void setRefined(bool v) { _was_refined = v; }
  inline const bool& wasRefined() const { return _was_refined; }

  inline size_t numFrames() const { return _f.size(); }

  inline void setFirstProjection(const Vec_<int,2>& x) { _x = x; }
  inline const Vec_<int,2>& getFirstProjection() const { return _x; }

  Vec3 _X;
  Vec3 _X_original;
  VisibilityList _f;
  ZnccPatchType _patch;
  std::vector<double> _descriptor;

  double _saliency  = 0.0;
  bool _was_refined = false;

  Vec_<int,2> _x;
}; // ScenePoint

PhotometricBundleAdjustment::PhotometricBundleAdjustment(
    const Calibration& calib, const ImageSize& image_size, const Options& options)
  : _calib(calib), _image_size(image_size), _options(options),
    _frame_buffer(options.slidingWindowSize)
{
  _mask.resize(_image_size.rows, _image_size.cols);
  _saliency_map.resize(_image_size.rows, _image_size.cols);

  _K_inv = calib.K().inverse();
}

PhotometricBundleAdjustment::~PhotometricBundleAdjustment() {}

static inline int PatchSizeFromRadius(int r) { return (2*r+1) * (2*r+1);  }

static inline int PatchRadiusFromLength(int l) { return std::sqrt(l)/2; }

template <typename T, class Image> static inline
void ExtractPatch(T* dst, const Image& I, const Vec_<int,2>& uv, int radius)
{
  int max_cols = I.cols() - radius - 1,
      max_rows = I.rows() - radius - 1;

  for(int r = -radius, i=0; r <= radius; ++r) {
    int r_i = std::max(radius, std::min(uv[1] + r, max_rows));
    for(int c = -radius; c <= radius; ++c, ++i) {
      int c_i = std::max(radius, std::min(uv[0] + c, max_cols));
      dst[i] = static_cast<T>( I(r_i, c_i) );
    }
  }
}

void PhotometricBundleAdjustment::
addFrame(const uint8_t* I_ptr, const float* Z_ptr, const Mat44& T, Result* result)
{
  _trajectory.push_back(T, _frame_id);
  const Eigen::Isometry3d T_w(_trajectory.back());
  const Eigen::Isometry3d T_c(T_w.inverse());

  typedef Eigen::Map<const Image_<uint8_t>, Eigen::Aligned> SrcMap;
  auto I = SrcMap(I_ptr, _image_size.rows, _image_size.cols);

  typedef Eigen::Map<const Image_<float>, Eigen::Aligned> SrcDepthMap;
  auto Z = SrcDepthMap(Z_ptr, _image_size.rows, _image_size.cols);

  auto frame = DescriptorFrame::Create(_frame_id, I, _options.descriptorType);

  auto B = std::max(_options.maskBlockRadius, std::max(2, _options.patchRadius));
  auto max_rows = (int) I.rows() - B - 1,
       max_cols = (int) I.cols() - B - 1,
       radius = _options.patchRadius,
       patch_length = PatchSizeFromRadius(radius),
       descriptor_dim = (int) frame->numChannels() * patch_length,
       mask_radius = _options.maskBlockRadius;

  //
  // Establish "correspondences" with the old data. This is the visibility list
  // computation
  //
  _mask.setOnes();

  int num_updated = 0, max_num_to_update = 0;
  for(size_t i = 0; i < _scene_points.size(); ++i) {
    const auto& pt = _scene_points[i];
    int f_dist = _frame_id - pt->lastFrameId();
    if(f_dist <= _options.maxFrameDistance) { // do not go too far back
      //
      // If the point projects to the current frame and it zncc score is
      // sufficiently highly, we'll add the current image to its visibility list
      Vec2 uv = _calib.project(T_c * pt->X());
      ++max_num_to_update;

      int r = std::round(uv[1]), c = std::round(uv[0]);
      if(r >= B && r < max_rows && c >= B && c <= max_cols) {
        typename ScenePoint::ZnccPatchType other_patch(I, uv);
        auto score = pt->patch().score( other_patch );
        if(score > _options.minScore) {
          num_updated++;

          // TODO update the patch for the new frame data
          pt->addFrame(_frame_id);

          //
          // block an area in the mask to prevent initializing redandant new
          // scene points
          //
          for(int r_i = -mask_radius; r_i <= mask_radius; ++r_i)
            for(int c_i = -mask_radius; c_i <= mask_radius; ++c_i)
              _mask(r+r_i, c+c_i) = 0;
        }
      }
    }
  }

  //
  // Add new scene points
  //
  decltype(_scene_points) new_scene_points;
  new_scene_points.reserve( max_rows * max_cols * 0.5 );
  frame->computeSaliencyMap(_saliency_map);

  typedef IsLocalMax_<decltype(_saliency_map), decltype(_mask)> IsLocalMax;
  const IsLocalMax is_local_max(_saliency_map, _mask, _options.nonMaxSuppRadius);

  for(int y = B; y < max_rows; ++y) {
    for(int x = B; x < max_cols; ++x) {
      auto z = Z(y,x);
      if(z >= _options.minValidDepth && z <= _options.maxValidDepth) {
        if(is_local_max(y, x)) {
          Vec3 X = T_w * (z * _K_inv * Vec3(x, y, 1.0));

          auto p = make_unique<ScenePoint>(X, _frame_id);
          Vec_<int,2> xy(x, y);
          p->setZnccPach( I, xy );
          p->descriptor().resize(descriptor_dim);
          p->setSaliency( _saliency_map(y,x) );
          p->setFirstProjection(xy);

          new_scene_points.push_back(std::move(p));
        }
      }
    }
  }

  //
  // keep the best N points
  //
  if(new_scene_points.size() > (size_t) _options.maxNumPoints) {
    auto nth = new_scene_points.begin() + _options.maxNumPoints;
    std::nth_element(new_scene_points.begin(), nth, new_scene_points.end(),
                     [&](const ScenePointPointer& a, const ScenePointPointer& b) {
                      return a->getSaliency() > b->getSaliency();
                     });
    new_scene_points.erase(nth, new_scene_points.end());
  }

  //
  // extract the descriptors
  //
  const int num_channels = frame->numChannels(),
        num_new_points = (int) new_scene_points.size();

  Info("updated %d [%0.2f%%] max %d new %d\n",
       num_updated, 100.0 * num_updated / _scene_points.size(),
       max_num_to_update, num_new_points);

  for(int k = 0; k < num_channels; ++k) {
    const auto& channel = frame->getChannel(k);
    for(int i = 0; i < num_new_points; ++i) {
      auto ptr = new_scene_points[i]->descriptor().data() + k*patch_length;
      ExtractPatch(ptr, channel, new_scene_points[i]->getFirstProjection(), radius);
    }
  }

  _scene_points.reserve(_scene_points.size() + new_scene_points.size());
  std::move(new_scene_points.begin(), new_scene_points.end(), std::back_inserter(_scene_points));

  _frame_buffer.push_back(DescriptorFramePointer(frame));

  if(_frame_buffer.full()) {
    optimize(result);
  }

  ++_frame_id;
}

static inline std::vector<double>
MakePatchWeights(int radius, bool do_gaussian, double s_x = 1.0,
                 double s_y = 1.0, double a = 1.0)
{
  int n = (2*radius + 1) * (2*radius + 1);

  if(do_gaussian) {
    std::vector<double> ret(n);
    double sum = 0.0;
    for(int r = -radius, i = 0; r <= radius; ++r) {
      const double d_r = (r*r) / s_x;
      for(int c = -radius; c <= radius; ++c, ++i) {
        const double d_c = (c*c) / s_y;
        const double w = a * std::exp( -0.5 * (d_r + d_c) );
        ret[i] = w;
        sum += w;
      }
    }

    for(int i = 0; i < n; ++i) {
      ret[i] /= sum;
    }

    return ret;
  } else {
    return std::vector<double>(n, 1.0);
  }
}

static Vec_<double,6> PoseToParams(const Mat44& T)
{
  Vec_<double,6> ret;
  const Mat_<double,3,3> R = T.block<3,3>(0,0);
  ceres::RotationMatrixToAngleAxis(ceres::ColumnMajorAdapter3x3(R.data()), ret.data());

  ret[3] = T(0,3);
  ret[4] = T(1,3);
  ret[5] = T(2,3);
  return ret;
}

static Mat_<double,4,4> ParamsToPose(const double* p)
{
  Mat_<double,3,3> R;
  ceres::AngleAxisToRotationMatrix(p, ceres::ColumnMajorAdapter3x3(R.data()));

  Mat_<double,4,4> ret(Mat_<double,4,4>::Identity());
  ret.block<3,3>(0,0) = R;
  ret.block<3,1>(0,3) = Vec_<double,3>(p[3], p[4], p[5]);
  return ret;
}

class PhotometricBundleAdjustment::DescriptorError
{
 public:
  /**
   * \param radius  the patch radius
   * \param calib   the camera calibration
   * \param p0      the reference frame descriptor must have size (2*radius+1)^2
   * \param frame   descriptor data of the image we are matching against
   */
  DescriptorError(const Calibration& calib, const std::vector<double>& p0,
                  const DescriptorFrame* frame, const std::vector<double>& w)
      : _radius(PatchRadiusFromLength(p0.size() / frame->numChannels())),
      _calib(calib), _p0(p0.data()), _frame(frame), _patch_weights(w.data())
  {
    // TODO should just pass the config to get the radius value
    assert( p0.size() == w.size() );
  }

  static ceres::CostFunction* Create(const Calibration& calib,
                                     const std::vector<double>& p0,
                                     const DescriptorFrame* f,
                                     const std::vector<double>& w)
  {
    return new ceres::AutoDiffCostFunction<DescriptorError, ceres::DYNAMIC, 6, 3>(
        new DescriptorError(calib, p0, f, w), p0.size());
  }

  template <class T> inline
  bool operator()(const T* const camera, const T* const point, T* residuals) const
  {
    T xw[3];
    ceres::AngleAxisRotatePoint(camera, point, xw);
    xw[0] += camera[3];
    xw[1] += camera[4];
    xw[2] += camera[5];

    T u_w, v_w;
    _calib.project(xw, u_w, v_w);

    for(size_t k = 0, i=0; k < _frame->numChannels(); ++k) {
      const auto& I = _frame->getChannel(k);
      const auto& G = _frame->getChannelGradient(k);
      const auto& Gx = G.Ix();
      const auto& Gy = G.Iy();

      for(int y = -_radius, j = 0; y <= _radius; ++y) {
        const T v = v_w + T(y);
        for(int x = -_radius; x <= _radius; ++x, ++i, ++j) {
          const T u = u_w + T(x);
          const T i0 = T(_p0[i]);
          const T i1 = SampleWithDerivative(I, Gx, Gy, u, v);
          residuals[i] = _patch_weights[j] * (i0 - i1);
        }
      }
    }

    // maybe we should return false if the point goes out of the image!
    return true;
  }

 private:
  const int _radius;
  const Calibration& _calib;
  const double* const _p0;
  const DescriptorFrame* _frame;
  const double* const _patch_weights;
}; // DescriptorError

static inline ceres::Solver::Options
GetSolverOptions(int num_threads, bool verbose = false, double tol = 1e-6)
{
  ceres::Solver::Options options;

  options.linear_solver_type            = ceres::SPARSE_SCHUR;

  options.minimizer_type                = ceres::TRUST_REGION;
  options.trust_region_strategy_type    = ceres::LEVENBERG_MARQUARDT;

  options.preconditioner_type           = ceres::CLUSTER_JACOBI;
  options.visibility_clustering_type    = ceres::SINGLE_LINKAGE;
  options.minimizer_progress_to_stdout  = verbose;
  options.max_num_iterations            = 500;

  options.num_threads = num_threads;
  options.num_linear_solver_threads = options.num_threads;

  options.function_tolerance  = tol;
  options.gradient_tolerance  = tol;
  options.parameter_tolerance = tol;

  return options;
}

void PhotometricBundleAdjustment::optimize(Result* result)
{
  auto frame_id_start = _frame_buffer.front()->id(),
       frame_id_end   = _frame_buffer.back()->id();

  auto patch_weights = MakePatchWeights(_options.patchRadius, _options.doGaussianWeighting);

  //
  // collect the camera poses in a single map for easy access
  //
  std::map<uint32_t, Vec_<double,6>> camera_params;
  for(auto id = frame_id_start; id <= frame_id_end; ++id) {
    // NOTE camera parameters are inverted
    camera_params[id] = PoseToParams(Eigen::Isometry3d(_trajectory.atId(id)).inverse().matrix());
  }

  //
  // get the points that we *should* optimize. They must have a large enough
  // visibility list
  //
  ceres::Problem problem;
  int num_selected_points = 0;
  for(auto& pt : _scene_points) {
    // it is enough to check the visibility list length, because we will remove
    // points as soon as they leave the optimization window
    if(pt->numFrames() >= 3 && pt->refFrameId() >= frame_id_start) {
      num_selected_points++;
      for(auto id : pt->visibilityList()) {
        if(id >= frame_id_start && id <= frame_id_end) {
          pt->setRefined(true);
          auto* camera_ptr = camera_params[id].data();
          auto* xyz = pt->X().data();

          const auto huber_t = _options.robustThreshold;
          auto* loss = huber_t > 0.0 ? new ceres::HuberLoss(huber_t) : nullptr;

          ceres::CostFunction* cost = nullptr;
          cost = DescriptorError::Create(_calib, pt->descriptor(), getFrameAtId(id), patch_weights);
          problem.AddResidualBlock(cost, loss, camera_ptr, xyz);
        }
      }
    }
  }

  // set the first camera cosntant
  {
    auto p = camera_params[frame_id_start].data();
    if(problem.HasParameterBlock(p)) {
      problem.SetParameterBlockConstant(p);
    } else {
      Warn("first camera is not in bundle\n");
    }
  }

  Info("Using %d points (%d residual blocks) [id start %d]\n",
       num_selected_points, problem.NumResidualBlocks(), frame_id_start);

  ceres::Solver::Summary summary;

#if HAS_OPENMP
  int num_threads = _options.numThreads > 0 ? _options.numThreads : std::min(omp_get_max_threads(), 4);
#else
  int num_threads = 4;
#endif

  ceres::Solve(GetSolverOptions(num_threads, _options.verbose), &problem, &summary);
  if(_options.verbose)
    std::cout << summary.FullReport() << std::endl;

  //
  // TODO: run another optimization pass over residuals with small error
  // (eliminate the outliers)
  //

  //
  // put back the refined camera poses
  //
  for(auto& it : camera_params) {
    _trajectory.atId(it.first) = Eigen::Isometry3d(
        ParamsToPose(it.second.data())).inverse().matrix();
  }

  //
  // set a side the old points. Since we are doing a sliding window, all points
  // at frame_id_start should go out
  //
  auto points_to_remove = removePointsAtFrame(frame_id_start);
  printf("removing %zu old points\n", points_to_remove.size());

  //
  // check if we should return a result to the user
  //
  if(result) {
    result->poses = _trajectory.poses();
    const auto npts = points_to_remove.size();
    result->refinedPoints.resize(npts);
    result->originalPoints.resize(npts);
    for(size_t i = 0; i < npts; ++i) {
      result->refinedPoints[i] = points_to_remove[i]->X();
      result->originalPoints[i] = points_to_remove[i]->getOriginalPoint();
    }

    result->initialCost = summary.initial_cost;
    result->finalCost   = summary.final_cost;
    result->fixedCost   = summary.fixed_cost;
    result->numSuccessfulStep = summary.num_successful_steps;
    result->totalTime = summary.total_time_in_seconds;
    result->numResiduals = summary.num_residuals;
    result->message = std::string(summary.message);
    result->iterationSummary = summary.iterations;
  }
}

auto PhotometricBundleAdjustment::getFrameAtId(uint32_t id) const -> const DescriptorFrame*
{
  for(const auto& f : _frame_buffer)
    if(f->id() == id) {
      return f.get();
    }

  throw std::runtime_error("could not find frame id!");
}

auto PhotometricBundleAdjustment::removePointsAtFrame(uint32_t id) -> ScenePointPointerList
{
  using namespace std;

  decltype(_scene_points) points_to_keep, points_to_remove;

  points_to_keep.reserve(_scene_points.size());
  points_to_remove.reserve( 0.5 * _scene_points.size() );

  partition_copy(make_move_iterator(begin(_scene_points)),
                 make_move_iterator(end(_scene_points)),
                 back_inserter(points_to_remove),
                 back_inserter(points_to_keep),
                 [&](const ScenePointPointer& p) { return p->refFrameId() <= id; });

  _scene_points.swap(points_to_keep);
  return points_to_remove;
}


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include <librcl/listener_compensation.hpp>
#include <libpanning/LoudspeakerArray.h>

#include <iostream>
#include <cstdio>
#include <stdlib.h>

#include <libvisr/signal_flow_context.hpp>
#include <libobjectmodel/object_vector.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/filesystem/path.hpp>

#include <stdexcept>
#include <string>

namespace visr
{
namespace rcl
{
namespace test
{

BOOST_AUTO_TEST_CASE( testListenerCompensation )
{
#if 0
  // TODO: Implement met
  LoudspeakerArray array;
  ListenerCompensation listcomp;
  FILE* file; //why as a pointer?
  Afloat(*compGains)[MAX_NUM_SPEAKERS];
  Afloat(*compDelays)[MAX_NUM_SPEAKERS];
  int j; //number of loudspeakers for printing out the gains and distances

  // 5.1 test, 2D Compensation

  boost::filesystem::path const basePath(CMAKE_SOURCE_DIR);
  boost::filesystem::path const arrayPath = basePath / boost::filesystem::path("config/isvr/stereo_audiolab.txt");
  BOOST_CHECK_MESSAGE( exists(arrayPath), "The loudspeaker array file does not exist.");

  file = fopen( arrayPath.string().c_str(), "r");
  BOOST_CHECK( array.load(file) != -1);
  if (file != 0)
  {
    fclose(file);
  }
  file = 0;

  listcomp.setLoudspeakerArray(&array);
  listcomp.setListenerPosition(1.0, 0.0, 0.0);

  listcomp.calcGainComp();// calculating delay and gain, this in the future will be combined in same function?
  listcomp.calcDelayComp();

  compGains = listcomp.getGains(); //getting the gains for the compensation
  compDelays = listcomp.getDelays(); //getting the gains for the compensation

  for (j = 0; j < listcomp.getNumSpeakers(); j++) {

    std::cout << "Source " << j <<"Gain=" << (*compGains)[j]<<"\n";
    std::cout << "Source " << j << "Delay=" <<(*compDelays)[j] << "\n";

  }
#endif
}

} // namespace test
} // namespace rcl
} // namespce visr


#pragma warning( disable: 4996 )
#include "Generic/common/leak_detection.h"
#include "Generic/common/SessionLogger.h"
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <wchar.h>

#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/thread/thread.hpp>

#include "Generic/common/ParamReader.h"
#include "Generic/common/StringTransliterator.h"
#include "Generic/common/UnexpectedInputException.h"
#include "Generic/common/foreach_pair.hpp"
#include "ActiveLearning/AnnotatedInstanceRecord.h"
#include "ActiveLearning/AnnotatedFeatureRecord.h"
#include "ActiveLearning/ActiveLearningData.h"
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/StringStore.h"
#include "ActiveLearning/strategies/ActiveLearningStrategy.h"
#include "ActiveLearning/strategies/RandomStrategy.h"
#include "ActiveLearning/strategies/CoverageStrategy.h"
#include "ActiveLearning/ui/ClientInstance.h"
#include "ActiveLearning/ui/JSONUtils.h"
#include "ActiveLearning/ui/MongooseUtils.h"
#include "ActiveLearning/alphabet/FeatureAlphabet.h"
#include "LearnIt/lbfgs/lbfgs.h"
#include "LearnIt/Eigen/Core"
#include "LearnIt/Eigen/Sparse"
#include "LearnIt/db/LearnIt2DB.h"
#include "OptimizationView.h"
#include "trainer.h"
#include "ProbChart.h"

using namespace Eigen;
using boost::make_shared;
using std::wstring; using std::string;

LearnIt2Trainer::LearnIt2Trainer(LearnIt2DB_ptr db, FeatureAlphabet_ptr alphabet,
	OptimizationView_ptr slotView, OptimizationView_ptr sentenceView,
	unsigned int n_instances, bool active_learning) :
		_db(db), _nParams(slotView->nParams()),
		_alphabet(alphabet), _sentenceView(sentenceView), _slotView(slotView),
		_canFinish(!active_learning),
		_iteration(0), _prob_chart(_slotView->data(), _sentenceView->data())
{
	if (db->readOnly()) {
		throw UnexpectedInputException("LearnIt2Trainer::LearnIt2Trainer",
			"Cannot construct LearnIt2Trainer from a read-only LearnIt DB");
	}
};

void LearnIt2Trainer::registerPostIterationHook(PostIterationCallback_ptr cb) {
	_postIterationHooks.push_back(cb);
}

void LearnIt2Trainer::syncToDB() {
	VectorXd tmpParams(_nParams);

	for (unsigned int i=0; i<_nParams; ++i) {
		if (_sentenceView->ownsFeature(i)) {
			tmpParams(i) = _sentenceView->parameters()(i);
		} else {
			tmpParams(i) = _slotView->parameters()(i);
		}
	}

	_alphabet->setFeatureWeights(tmpParams);
}

void LearnIt2Trainer::optimize() {
	optimize(ParamReader::getRequiredIntParam("max_trainer_iterations"));
}

void LearnIt2Trainer::optimize(int max_iterations) {
	int max_view_iterations = ParamReader::getOptionalIntParamWithDefaultValue(
			"max_view_iterations", 1000);
	double global_objective_change_threshold = ParamReader::getOptionalFloatParamWithDefaultValue(
			"global_objective_change_threshold", 0.01);
	double lbfgs_gradient_tolerance = ParamReader::getOptionalFloatParamWithDefaultValue(
			"lbfgs_gradient_tolerance", 1e-3);
	double lbfgs_objective_tolerance = ParamReader::getOptionalFloatParamWithDefaultValue(
			"lbfgs_objective_tolerance", 0.0);
	int outer_iterations_after_can_finish = ParamReader::getOptionalIntParamWithDefaultValue(
			"iterations_after_annotation", 25);

	SessionLogger::info("remember_to_cite") << L"Remember to cite "
		<< L"Massih-Reza Amini & Cyril Goutte. \"A co-classification "
		<< L"approach to learning from multilingual corpora.\". Machine Learning"
		<< L" (2010) 79:105-121";

	int iterations_after_annotation = 0;
	while (!_canFinish) {
		double old_global_objective = -std::numeric_limits<double>::max();

		_sentenceView->inference();
		_slotView->inference();
		_sentenceView->optimize(max_view_iterations, lbfgs_gradient_tolerance,
				lbfgs_objective_tolerance);
		_slotView->optimize(max_view_iterations, lbfgs_gradient_tolerance,
				lbfgs_objective_tolerance);
		_global_objective = _sentenceView->value() + _slotView->value();

		SessionLogger::info("global_progress") <<
			_iteration << L": g: " << _global_objective << L"; sent: " <<
			_sentenceView->value() << "; slot: " << _slotView->value();

		while (fabs(_global_objective - old_global_objective) > global_objective_change_threshold) {
			if (iterations_after_annotation > outer_iterations_after_can_finish) {
				break;
			}
			++_iteration;
			if (_canFinish) {
				++iterations_after_annotation;
			}
			postIterationHook();
			_sentenceView->optimize(max_view_iterations, lbfgs_gradient_tolerance,
					lbfgs_objective_tolerance);
			_sentenceView->inference();
			_slotView->optimize(max_view_iterations, lbfgs_gradient_tolerance,
					lbfgs_objective_tolerance);
			_slotView->inference();
			old_global_objective = _global_objective;
			_global_objective = _sentenceView->value() + _slotView->value();
			SessionLogger::info("global_progress") <<
			_iteration << L"(" << iterations_after_annotation << L"): g: " << _global_objective << L"; sent: " <<
			_sentenceView->value() << "; slot: " << _slotView->value();
		}
	}
}

bool LearnIt2Trainer::postIterationHook() {
	bool ret = false;

	BOOST_FOREACH(PostIterationCallback_ptr callback, _postIterationHooks) {
		if ((*callback)()) {
			setDirty();
			ret = true;
		}
	}

	_sentenceView->betweenIterations();
	_slotView->betweenIterations();

	_prob_chart.observe();

	++_iteration;
	return ret;
}

void LearnIt2Trainer::setDirty() {
	_sentenceView->setDirty();
	_slotView->setDirty();
}

int LearnIt2Trainer::nFeatures() const {
	return _nParams;
}

LearnIt2DB& LearnIt2Trainer::database() const {
	return *_db;
}

wstring LearnIt2Trainer::UIDiagnosticString() const {
	wstringstream str;
	str << L"Sent View: " << _sentenceView->UIDiagnosticString() << L"
";
	str << L"Slot View: " << _slotView->UIDiagnosticString() << L"
";
	str << L"Global: " << _global_objective;

	return str.str();
}

const ProbChart& LearnIt2Trainer::probChart() const {
	return _prob_chart;
}

boost::mutex& LearnIt2Trainer::updateMutex() {
	return _active_learning_update_mutex;
}

void LearnIt2Trainer::stop() {
	_canFinish = true;
}

double LearnIt2Trainer::featureWeight(int feat) const {
	if (_sentenceView->ownsFeature(feat)) {
		return _sentenceView->parameters()(feat);
	} else {
		return _slotView->parameters()(feat);
	}
}

std::wstring LearnIt2Trainer::featureName(int feat) const {
	return _alphabet->getFeatureName(feat);
}

void LearnIt2Trainer::_test_clear_objective_components() {
	_sentenceView->_test_clear_objective_components();
	_slotView->_test_clear_objective_components();
}


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                         (C) 2007 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/phonon/IncoherentElastic.h"
#include "mccomponents/boostpython_binding/wrap_kernel.h"

namespace wrap_mccomponents {

  void wrap_Phonon_IncoherentElastic_kernel()
  {
    using namespace boost::python;
    using namespace mccomponents::boostpython_binding;

    typedef mccomponents::kernels::phonon::IncoherentElastic w_t;

    kernel_wrapper<w_t>::wrap
      ( "Phonon_IncoherentElastic_kernel",
	init<
	const w_t::atoms_t &, // atoms
	w_t::float_t, // unitcell_vol. AA**3
	w_t::float_t, // dw_core. AA**2
	w_t::float_t, // scattering_xs. barn
	w_t::float_t  // absorption_xs. barn
	> ()
	[with_custodian_and_ward<1,2> () ]
	)
      ;

  }

}

// version
// $Id: wrap_Phonon_IncoherentElastic_kernel.cc 603 2010-10-04 15:58:16Z linjiao $

// End of file


#include "Generator.hpp"

#include <boost/test/unit_test.hpp>

namespace tbu=toolbox::util;

struct some_resource {
    int x;
    explicit some_resource(int x) : x(x) {
        std::printf("some_resource(%i)\n", x);
    }
    ~some_resource() {
        std::printf("~some_resource(%i)\n", x);
    }
};

static tbu::generator<const uint64_t> fib(int max) {
    some_resource r{2};
    auto a = 0, b = 1 ;
    for(auto n = 0; n <  max; n ++)  {
        co_yield b;
        const auto next = a + b;
        a = b, b = next;
    }
}

static tbu::generator<const uint64_t> test(int max) {
    auto g = fib(max);
    some_resource r{1};
    co_yield std::move(g);
}

BOOST_AUTO_TEST_SUITE(GeneratorSuite)

BOOST_AUTO_TEST_CASE(GeneratorIterate)
{
    //setbuf(stdout, NULL);

#if __has_include(<ranges>)
    auto v = test(10) | std::views::drop(9);
    return *std::ranges::begin(v);
#else
    uint64_t  i = 0;
    for(auto a : test(10)) {
        i = a;
    }
    return i;
#endif
}
BOOST_AUTO_TEST_SUITE_END()


#include <string>
#include <set>
#include <regex>
#include <iostream>
#include <algorithm>
#include <boost/program_options.hpp>
#include "JavaRandom.hpp"
#include "minecrack-48bit.hpp"

using namespace std;

/*
 * Get exceptions instead of asserts from boost.
 */
namespace boost {
void assertion_failed(const char* expr, const char* function, const char* file, long line) {
	throw logic_error("Boost assert failed: "s + expr + ", at " + file + ":" + to_string(line) + " in " + function);
}

void assertion_failed_msg(const char* expr, const char* msg, const char* function, const char* file, long line) {
	throw logic_error(
			"Boost assert failed ("s + msg + "): " + "" + expr + ", at " + file + ":" + to_string(line) + " in " +
			function);
}
}

namespace cmdline {

bool verbose;
std::vector<int64_t> chunk_seed_offset;

}

int main(int argc, char** argv) try {
	{
		using namespace boost::program_options;
		options_description options;
		options.add_options()
				("verbose,v", "print extra informations")
				("force,f", "do not bail out if too few chunks are provided");
		if (argc == 1) {
			cerr << "Usage: " << argv[0] << " [options] chunk1X:chunk1Z chunk2X:chunk2Z ..." << endl << "Options:"
			     << endl << options;
			return 0;
		}
		options_description options_with_chunks;
		options_with_chunks.add(options).add_options()("slime-chunk", value<vector<string>>());
		positional_options_description positional;
		positional.add("slime-chunk", -1);
		variables_map vm;
		try {
			store(command_line_parser(argc, argv).options(options_with_chunks).positional(positional).run(),
					vm);
			notify(vm);
		} catch (const boost::program_options::error& e) { throw invalid_argument(e.what()); }
		cmdline::verbose = vm.count("verbose");
		if (!vm.count("slime-chunk"))
			throw invalid_argument("You must specify some slime chunks");
		auto slimechunks_vector = vm["slime-chunk"].as<vector<string>>();
		set<string> slimechunks(slimechunks_vector.begin(), slimechunks_vector.end());
		regex slimechunk_regex("([\\-\\+]?\\d+)\\:([\\-\\+]?\\d+)", regex::ECMAScript | regex::optimize);
		smatch result;
		for (auto& chunkspec: slimechunks) {
			if (!regex_match(chunkspec, result, slimechunk_regex))
				throw invalid_argument("slime chunk coordinate " + chunkspec + " cannot be parsed");
			try {
				int chunkX = stoi(result[1]), chunkZ = stoi(result[2]);
				cmdline::chunk_seed_offset.push_back(slime_seed_offset(chunkX, chunkZ));
			} catch (const out_of_range& o) {
				throw invalid_argument("slime chunk coordinate " + chunkspec + " cannot is out of int range");
			}
		}
		/*
		 * each slime chunk gives log2(10) bits of information on the 48bit seed of the world. At least 15 slime
		 * chunks must be found to have one or just few 48bit candidates.
		 */
		if (cmdline::chunk_seed_offset.size() < 13 && !vm.count("force"))
			throw invalid_argument("Too few slime chunks provided, at least 13 are needed (or use the -f option)");
		if (cmdline::chunk_seed_offset.size() < 15)
			cerr << "Warning: 15 or more slime chunks should be provided" << endl;
	}

	//calculate lower 18bits candidates
	vector<uint32_t> lowbits_candidates = ::lowbits_candidates();
	if (lowbits_candidates.empty())
		throw invalid_argument("The provided slime chunks are wrong or they are generated by a modded Minecraft version");
	if (cmdline::verbose) {
		cerr << "Found " << lowbits_candidates.size() << " lowbits candidates:";
		for (auto c: lowbits_candidates) cerr << ' ' << c;
		cerr << endl;
	}

	auto seeds = test_seeds(lowbits_candidates);
	sort(seeds.begin(), seeds.end());
	for (auto s: seeds) cout << s << endl;

} catch (const invalid_argument& e) {
	cerr << "Invalid argument: " << e.what() << endl;
	return 1;
} catch (const exception& e) {
	cerr << e.what() << endl;
	return 2;
}


/**
 * @file numpdesetup_test.cc
 * @brief NPDE homework NumPDESetup code
 * @author Ralf Hiptmair, Oliver Rietmann, Erick Schulz
 * @date 17.02.2020
 * @copyright Developed at ETH Zurich
 */

#include "../numpdesetup.h"

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <iostream>

namespace NumPDESetup::test {

/* SAM_LISTING_BEGIN_3 */
TEST(NumPDESetup, dummyFunction) {
  double x = 1.0;
  int n = 2;
  // Invoke the function to be tested
  Eigen::Vector2d v = NumPDESetup::dummyFunction(x, n);
  // The expected solution
  Eigen::Vector2d v_ref = {1.0, 1.0};
  // Check whether function call produces correct result using the Google test
  // framework
  double tol = 1.0e-8;
  ASSERT_NEAR(0.0, (v - v_ref).lpNorm<Eigen::Infinity>(), tol);
}
/* SAM_LISTING_END_3 */

}  // namespace NumPDESetup::test


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_IS_CONVEX_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_IS_CONVEX_SPHERICAL_HPP

#include <boost/geometry/strategies/convex_hull/spherical.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/is_convex/services.hpp>
#include <boost/geometry/strategies/spherical/point_in_point.hpp>
#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace is_convex
{

template <typename CalculationType = void>
class spherical : public strategies::convex_hull::spherical<CalculationType>
{
public:
    template <typename Geometry1, typename Geometry2>
    static auto relate(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                             && util::is_pointlike<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::spherical_point_point();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, spherical_equatorial_tag>
{
    using type = strategies::is_convex::spherical<>;
};

template <typename CT>
struct strategy_converter<strategy::side::spherical_side_formula<CT>>
{
    static auto get(strategy::side::spherical_side_formula<CT> const& )
    {
        return strategies::is_convex::spherical<CT>();
    }
};

} // namespace services

}} // namespace strategies::is_convex

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_IS_CONVEX_SPHERICAL_HPP