// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "nucleon.h"

#include <cmath>
#include <limits>
#include <random>
#include <stdexcept>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iomanip>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/filesystem.hpp>

#include "fwd_decl.h"

namespace trento {

namespace {

// These constants define distances in terms of the width of the nucleon profile
// Gaussian thickness function.

// Truncation radius of the thickness function.
constexpr double max_radius_widths = 5.;

// Maximum impact parameter for participation.
constexpr double max_impact_widths = 6.;
// Create ctor parameters for unit mean std::gamma_distribution.
//   mean = alpha*beta == 1  ->  beta = 1/alpha
// Used below in NucleonProfile ctor initializer list.

template <typename RealType> using param_type =
  typename std::gamma_distribution<RealType>::param_type;

template <typename RealType>
param_type<RealType> gamma_param_unit_mean(RealType alpha = 1.) {
  return param_type<RealType>{alpha, 1./alpha};
}

// Return appropriate path to temporary file directory.
// Used to store cross section parameter \sigma_partonic.
fs::path get_data_home() {
  const auto data_path = std::getenv("XDG_DATA_HOME");
  if(data_path == nullptr)
    return fs::path{std::getenv("HOME")} / ".local/share";
  return data_path;
}

// Test approximate cross section parameter equality.
bool almost_equal(double a, double b) {
  return fabs(a - b) < 1e-6;
}

// Calculate constituent position sampling width from CL arguments
double calc_sampling_width(const VarMap& var_map) {
  auto nucleon_width = var_map["nucleon-width"].as<double>();
  auto constituent_width = var_map["constit-width"].as<double>();
  auto constituent_number = var_map["constit-number"].as<int>();

  auto one_constituent = (constituent_number == 1);
  auto same_size = (nucleon_width - constituent_width < 1e-6);

  if (one_constituent || same_size)
    return 0.0;
  else {
    auto num = sqr(nucleon_width) - sqr(constituent_width);
    auto denom = 1. - 1./constituent_number;
    return std::sqrt(num / denom);
  }
}

// Determine cross section parameter for sampling nucleon participants.
// This semi-analytic method is only valid for one constituent.
double analytic_partonic_cross_section(const VarMap& var_map) {
  // Read parameters from the configuration.
  auto sigma_nn = var_map["cross-section"].as<double>();
  auto width = var_map["nucleon-width"].as<double>();

  // TODO: automatically set from beam energy
  // if (sigma_nn < 0.) {
  //   auto sqrt_s = var_map["beam-energy"].as<double>();
  // }

  // Initialize arguments for boost root finding function.

  // Bracket min and max.
  auto a = -10.;
  auto b = 20.;

  // Tolerance function.
  // Require 3/4 of double precision.
  math::tools::eps_tolerance<double> tol{
    (std::numeric_limits<double>::digits * 3) / 4};

  // Maximum iterations.
  // This is overkill -- in testing only 10-20 iterations were required
  // (but no harm in overestimating).
  boost::uintmax_t max_iter = 1000;

  // The right-hand side of the equation.
  auto rhs = sigma_nn / (4 * math::double_constants::pi * sqr(width));

  // This quantity appears a couple times in the equation.
  auto c = sqr(max_impact_widths) / 4;

  try {
    auto result = math::tools::toms748_solve(
      [&rhs, &c](double x) {
        using std::exp;
        using math::expint;
        return c - expint(-exp(x)) + expint(-exp(x-c)) - rhs;
      },
      a, b, tol, max_iter);

    auto cross_section_param = .5*(result.first + result.second);
    return std::exp(cross_section_param) * 4 * math::double_constants::pi * sqr(width);
  }
  catch (const std::domain_error&) {
    // Root finding fails for very small nucleon widths, w^2/sigma_nn < ~0.01.
    throw std::domain_error{
      "unable to fit cross section -- nucleon width too small?"};
  }
}

// Determine cross section parameter for sampling nucleon participants.
// This Monte Carlo numeric method is used for more than one constituent.
double numeric_partonic_cross_section(const VarMap& var_map) {
  MonteCarloCrossSection mc_cross_section(var_map);
  auto sigma_nn = var_map["cross-section"].as<double>();
  auto width = var_map["constit-width"].as<double>();

  // Bracket min and max.
  auto a = -10.;
  auto b = 20.;

  // Tolerance function.
  math::tools::eps_tolerance<double> tol{8};

  // Maximum iterations.
  boost::uintmax_t max_iter = 20;

  // Dimensional constant [fm^-1] used to rescale sigma_partonic
  auto c = 4 * math::double_constants::pi * sqr(width);

  try {
    auto result = math::tools::toms748_solve(
      [&sigma_nn, &mc_cross_section, &c](double cross_section_param) {
        auto x = std::exp(cross_section_param) * c;
        return mc_cross_section(x) - sigma_nn;
      },
      a, b, tol, max_iter);

    auto cross_section_param = .5*(result.first + result.second);
    return std::exp(cross_section_param) * c;
  }
  catch (const std::domain_error&) {
    // Root finding fails for very small nucleon widths, w^2/sigma_nn < ~0.01.
    throw std::domain_error{
      "unable to fit cross section -- nucleon and/or constituent width too small?"};
  }
}

// Determine the cross section parameter given the nucleon_width,
// constituent_width, constituent number, and inelastic cross section.
double partonic_cross_section(const VarMap& var_map) {
  // Read parameters from the configuration.
  auto nucleon_width = var_map["nucleon-width"].as<double>();
  auto constituent_width = var_map["constit-width"].as<double>();
  auto constituent_number = var_map["constit-number"].as<int>();
  auto sigma_nn = var_map["cross-section"].as<double>();

  // Cross section parameters
  double nucleon_width_;
  double constituent_width_;
  int constituent_number_;
  double sigma_nn_;
  double sigma_partonic;

  // Establish trento cache path
  auto cache_dir = get_data_home() / "trento";
  auto cache_path = cache_dir / "cross_section.dat";
  fs::create_directory(cache_dir);

  // Check if cache exists
  if (fs::exists(cache_path.string())){

    // Open cache read only
    std::fstream cache_read(cache_path.string(), std::ios_base::in);

    // Check cache for previously tabulated cross section parameters
    while(!cache_read.eof()) {
      cache_read >> constituent_number_ >> nucleon_width_ >>
        constituent_width_ >> sigma_nn_ >> sigma_partonic;
      if (almost_equal(nucleon_width, nucleon_width_) &&
          almost_equal(constituent_width, constituent_width_) &&
          almost_equal(sigma_nn, sigma_nn_) &&
          (constituent_number == constituent_number_)) {
        return sigma_partonic;
      }
    }

    // Close read-only file stream
    cache_read.close();
  }

  // Open cache with write access
  std::fstream cache_write(cache_path.string(),
      std::ios_base::out | std::ios_base::app);

  // Use numeric method to determine sigma_partonic if there is nucleon
  // substructure, otherwise use semi-analytic method.
  if (constituent_number > 1)
    sigma_partonic = numeric_partonic_cross_section(var_map);
  else
    sigma_partonic = analytic_partonic_cross_section(var_map);

  // Save new cross section parameters to cache
  using std::fixed;
  using std::setprecision;
  using std::setw;
  using std::scientific;
  using std::endl;

  cache_write << setprecision(6)
              << std::left << setw(4) << fixed << constituent_number
              << std::right << setw(10) << fixed << nucleon_width
              << std::right << setw(10) << fixed << constituent_width
              << std::right << setw(10) << fixed << sigma_nn
              << std::right << setw(14) << scientific << sigma_partonic
              << endl;

  return sigma_partonic;
}

}  // unnamed namespace

NucleonCommon::NucleonCommon(const VarMap& var_map)
    : fast_exp_(-.5*sqr(max_radius_widths), 0., 1000),
      nucleon_width_(var_map["nucleon-width"].as<double>()),
      constituent_width_(var_map["constit-width"].as<double>()),
      constituent_number_(std::size_t(var_map["constit-number"].as<int>())),
      sampling_width_(calc_sampling_width(var_map)),
      max_impact_sq_(sqr(max_impact_widths*nucleon_width_)),
      constituent_width_sq_(sqr(constituent_width_)),
      constituent_radius_sq_(sqr(max_radius_widths*constituent_width_)),
      sigma_partonic_(partonic_cross_section(var_map)),
      prefactor_(math::double_constants::one_div_two_pi/constituent_width_sq_/constituent_number_),
      calc_ncoll_(var_map["ncoll"].as<bool>()),
      participant_fluctuation_dist_(gamma_param_unit_mean(var_map["fluctuation"].as<double>())),
      constituent_position_dist_(0, sampling_width_)
{}

MonteCarloCrossSection::MonteCarloCrossSection(const VarMap& var_map)
  : nucleon_width_(var_map["nucleon-width"].as<double>()),
    constituent_width_(var_map["constit-width"].as<double>()),
    constituent_number_(std::size_t(var_map["constit-number"].as<int>())),
    sampling_width_(calc_sampling_width(var_map)),
    max_impact_(max_impact_widths*nucleon_width_),
    constituent_width_sq_(sqr(constituent_width_)),
    prefactor_(math::double_constants::one_div_two_pi/constituent_width_sq_/constituent_number_)
{}

double MonteCarloCrossSection::operator() (const double sigma_partonic) const {

  random::CyclicNormal<> cyclic_normal{
    0., sampling_width_, cache_size, n_loops
  };

  struct Constituent {
    double x, y;
  };

  std::vector<Constituent> nucleonA(constituent_number_);
  std::vector<Constituent> nucleonB(constituent_number_);

  auto max_impact_sq_ = sqr(max_impact_);
  double ref_cross_section = 0.;
  double prob_miss = 0.;
  int pass_tolerance = 0;

  for (std::size_t n = 0; n < n_max; ++n) {
    // Sample b from P(b)db = 2*pi*b.
    auto b = max_impact_ * std::sqrt(random::canonical<double>());

    for (auto&& q : nucleonA) {
      q.x = cyclic_normal(random::engine);
      q.y = cyclic_normal(random::engine);
    }

    for (auto&& q : nucleonB) {
      q.x = cyclic_normal(random::engine);
      q.y = cyclic_normal(random::engine);
    }

    auto overlap = 0.;
    for (auto&& qA : nucleonA) {
      for (auto&& qB : nucleonB) {
        auto distance_sq = sqr(qA.x - qB.x + b) + sqr(qA.y - qB.y);
        overlap += std::exp(-.25*distance_sq/constituent_width_sq_);
      }
    }

    prob_miss +=
      std::exp(-sigma_partonic * prefactor_/(2.*constituent_number_) * overlap);

    auto prob_hit = 1. - (prob_miss/n);
    auto cross_section = M_PI*max_impact_sq_*prob_hit;

    auto update_difference = std::abs(cross_section - ref_cross_section);

    if (update_difference < tolerance) {
      ++pass_tolerance;
    } else {
      pass_tolerance = 0;
      ref_cross_section = cross_section;
    }

    if (pass_tolerance > n_pass){
      return cross_section;
    }
  }

  throw std::out_of_range{
    "Partonic cross section failed to converge \
      -- check nucleon width, constituent width, and constituent number"};
}

}  // namespace trento


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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 NETKET_ADAMAX_HPP
#define NETKET_ADAMAX_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class AdaMax : public AbstractOptimizer {
  int npar_;

  double alpha_;
  double beta1_;
  double beta2_;

  Eigen::VectorXd ut_;
  Eigen::VectorXd mt_;

  double niter_;
  double niter_reset_;

  double epscut_;

  const Complex I_;

 public:
  explicit AdaMax(double alpha = 0.001, double beta1 = 0.9,
                  double beta2 = 0.999, double epscut = 1.0e-7)
      : alpha_(alpha), beta1_(beta1), beta2_(beta2), epscut_(epscut), I_(0, 1) {
    npar_ = -1;
    niter_ = 0;
    niter_reset_ = -1;

    PrintParameters();
  }

  // TODO remove
  // Json constructor
  explicit AdaMax(const json &pars) : I_(0, 1) {
    npar_ = -1;
    niter_ = 0;
    niter_reset_ = -1;

    from_json(pars);
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Adamax optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Alpha = " << alpha_ << std::endl;
    InfoMessage() << "Beta1 = " << beta1_ << std::endl;
    InfoMessage() << "Beta2 = " << beta2_ << std::endl;
    InfoMessage() << "Epscut = " << epscut_ << std::endl;
  }

  void Init(const Eigen::VectorXd &pars) override {
    npar_ = pars.size();
    ut_.setZero(npar_);
    mt_.setZero(npar_);

    niter_ = 0;
  }

  void Init(const Eigen::VectorXcd &pars) override {
    npar_ = 2 * pars.size();
    ut_.setZero(npar_);
    mt_.setZero(npar_);

    niter_ = 0;
  }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) override {
    assert(npar_ > 0);

    mt_ = beta1_ * mt_ + (1. - beta1_) * grad;

    for (int i = 0; i < npar_; i++) {
      ut_(i) = std::max(std::max(std::abs(grad(i)), beta2_ * ut_(i)), epscut_);
    }
    niter_ += 1.;
    if (niter_reset_ > 0) {
      if (niter_ > niter_reset_) {
        niter_ = 1;
      }
    }

    double eta = alpha_ / (1. - std::pow(beta1_, niter_));
    for (int i = 0; i < npar_; i++) {
      pars(i) -= eta * mt_(i) / ut_(i);
    }
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXd &pars) override {
    Update(Eigen::VectorXd(grad.real()), pars);
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXcd &pars) override {
    assert(npar_ == 2 * pars.size());

    for (int i = 0; i < pars.size(); i++) {
      mt_(2 * i) = beta1_ * mt_(2 * i) + (1. - beta1_) * grad(i).real();
      mt_(2 * i + 1) = beta1_ * mt_(2 * i + 1) + (1. - beta1_) * grad(i).imag();
    }

    for (int i = 0; i < pars.size(); i++) {
      ut_(2 * i) = std::max(
          std::max(std::abs(grad(i).real()), beta2_ * ut_(2 * i)), epscut_);
      ut_(2 * i + 1) = std::max(
          std::max(std::abs(grad(i).imag()), beta2_ * ut_(2 * i + 1)), epscut_);
    }

    niter_ += 1.;
    if (niter_reset_ > 0) {
      if (niter_ > niter_reset_) {
        niter_ = 1;
      }
    }

    double eta = alpha_ / (1. - std::pow(beta1_, niter_));
    for (int i = 0; i < pars.size(); i++) {
      pars(i) -= eta * mt_(2 * i) / ut_(2 * i);
      pars(i) -= eta * I_ * mt_(2 * i + 1) / ut_(2 * i + 1);
    }
  }

  void Reset() override {
    ut_ = Eigen::VectorXd::Zero(npar_);
    mt_ = Eigen::VectorXd::Zero(npar_);
    niter_ = 0;
  }

  void SetResetEvery(double niter_reset) { niter_reset_ = niter_reset; }

  void from_json(const json &pars) {
    // DEPRECATED (to remove for v2.0.0)
    std::string section = "Optimizer";
    if (!FieldExists(pars, section)) {
      section = "Learning";
    }

    alpha_ = FieldOrDefaultVal(pars[section], "Alpha", 0.001);
    beta1_ = FieldOrDefaultVal(pars[section], "Beta1", 0.9);
    beta2_ = FieldOrDefaultVal(pars[section], "Beta2", 0.999);
    epscut_ = FieldOrDefaultVal(pars[section], "Epscut", 1.0e-7);
  }
};

}  // namespace netket

#endif


#include "drake/solvers/unrevised_lemke_solver.h"

#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <limits>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <utility>
#include <vector>

#include <Eigen/LU>

#include "drake/common/autodiff.h"
#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/never_destroyed.h"
#include "drake/common/text_logging.h"
#include "drake/common/unused.h"

using drake::log;

namespace drake {
namespace solvers {

namespace {

// A linear system solver that accommodates the inability of the LU
// factorization to be AutoDiff'd (true in Eigen 3, at least). For double types,
// the faster LU factorization and solve is used. For other types, QR
// factorization and solve is used.
template <class T>
class LinearSolver {
 public:
  explicit LinearSolver(const MatrixX<T>& m);

  VectorX<T> Solve(const VectorX<T>& v) const;

 private:
  Eigen::ColPivHouseholderQR<MatrixX<T>> qr_;
  Eigen::PartialPivLU<MatrixX<double>> lu_;
};

template <class T>
LinearSolver<T>::LinearSolver(const MatrixX<T>& M) {
  if (M.rows() > 0)
    qr_ = Eigen::ColPivHouseholderQR<MatrixX<T>>(M);
}

template <>
LinearSolver<double>::LinearSolver(
    const MatrixX<double>& M) {
  if (M.rows() > 0)
    lu_ = Eigen::PartialPivLU<MatrixX<double>>(M);
}

template <class T>
VectorX<T> LinearSolver<T>::Solve(
    const VectorX<T>& v) const {
  if (v.rows() == 0) {
    DRAKE_DEMAND(qr_.rows() == 0);
    return VectorX<T>(0);
  }
  return qr_.solve(v);
}

template <>
VectorX<double> LinearSolver<double>::Solve(
    const VectorX<double>& v) const {
  if (v.rows() == 0) {
    DRAKE_DEMAND(lu_.rows() == 0);
    return VectorX<double>(0);
  }
  return lu_.solve(v);
}
}  // anonymous namespace

template <>
void UnrevisedLemkeSolver<AutoDiffXd>::DoSolve(
    const MathematicalProgram&, const Eigen::VectorXd&,
    const SolverOptions&, MathematicalProgramResult*) const {
  throw std::logic_error(
      "UnrevisedLemkeSolver cannot yet be used in a "
      "MathematicalProgram while templatized as an AutoDiff");
}

template <typename T>
void UnrevisedLemkeSolver<T>::DoSolve(
    const MathematicalProgram& prog,
    const Eigen::VectorXd& initial_guess,
    const SolverOptions& merged_options,
    MathematicalProgramResult* result) const {
  if (!prog.GetVariableScaling().empty()) {
    static const logging::Warn log_once(
      "UnrevisedLemkeSolver doesn't support the feature of variable scaling.");
  }

  unused(initial_guess);
  unused(merged_options);

  // Solve each individual LCP, writing the result back to the decision
  // variables through the binding and returning true iff all LCPs are
  // feasible.
  //
  // If any is infeasible, returns false and does not alter the decision
  // variables.

  // Create a dummy variable for the number of pivots used.
  int num_pivots = 0;
  const auto& bindings = prog.linear_complementarity_constraints();
  Eigen::VectorXd x_sol(prog.num_vars());
  for (const auto& binding : bindings) {
    Eigen::VectorXd constraint_solution(binding.GetNumElements());
    const std::shared_ptr<LinearComplementarityConstraint> constraint =
        binding.evaluator();
    bool solved = SolveLcpLemke(
        constraint->M(), constraint->q(), &constraint_solution, &num_pivots);
    if (!solved) {
      result->set_solution_result(SolutionResult::kUnknownError);
      return;
    }
    for (int i = 0; i < binding.evaluator()->num_vars(); ++i) {
      const int variable_index =
          prog.FindDecisionVariableIndex(binding.variables()(i));
      x_sol(variable_index) = constraint_solution(i);
    }
  }
  result->set_optimal_cost(0.0);
  result->set_x_val(x_sol);
  result->set_solution_result(SolutionResult::kSolutionFound);
}

// Utility function for copying an r-dimensional column vector v (designated by
// the indices in row_indices and col_index) from a matrix M to a target
// vector, `out`. Let M be the matrix `in` augmented with a single column of
// ones (i.e., the "covering vector"); put another way, M = | in 1 |, where "in"
// refers the n × m-dimensional matrix `in` and 1 is a column of ones (so M is
// n × (m+1)-dimensional). Let R be a r × n "row selection" matrix, constructed
// using `row_indices`. R is constructed using r = `row_indices.size()` as
// follows:
//
// Rᵢⱼ = { 1  if j = row_indices[i], ∀i ∈ 0..r-1, ∀j ∈ 0..n-1
//       { 0  otherwise
//
// Consider the following example with `in` set to the 3 × 3 identity matrix,
// `row_indices = { 2, 0}` and `col_index = 1`. This would make:
// R =  | 0 0 1 |      and R⋅M = | 0 0 1 1 |
//      | 1 0 0 |                | 1 0 0 1 |
//
// The second column (`col_index = 1`) of R⋅M is v = | 0 |
//                                                   | 0 |
//
// `row_indices` need not be in sorted order, and `out` need not be properly
// sized on entry (it will be resized as necessary). However, this method aborts
// if any element of `row_indices` is out of range, `col_index` is out of range,
// *or* if there are any duplicated elements in `row_indices`.
template <class T>
void UnrevisedLemkeSolver<T>::SelectSubColumnWithCovering(
    const MatrixX<T>& in,
    const std::vector<int>& row_indices,
    int col_index, VectorX<T>* out) {
  DRAKE_ASSERT(ValidateIndices(row_indices, in.rows()));

  const int num_rows = row_indices.size();
  out->resize(num_rows);

  // Look for the covering vector first.
  if (col_index == in.cols()) {
    out->setOnes();
    return;
  }

  DRAKE_DEMAND(0 <= col_index && col_index < in.cols());
  const auto in_column = in.col(col_index);
  for (int i = 0; i < num_rows; i++) {
    DRAKE_ASSERT(row_indices[i] < in_column.size());
    (*out)[i] = in_column[row_indices[i]];
  }
}

// Utility function for copying an r-dimensional column vector v (designated by
// the indices in `row_indices`) from n-dimensional vector `in` to a target
// vector, `out`. Let R be a r × n "row selection" matrix, constructed using
// r = `row_indices.size()` as follows:
//
// Rᵢⱼ = { 1  if j = row_indices[i], ∀i ∈ 0..r-1, ∀j ∈ 0..n-1
//       { 0  otherwise
//
// Consider the following example with `in` set to the vector [ 1 2 3 ]ᵀ,
// and `row_indices = { 2, 0}`. This would make:
// R =  | 0 0 1 |      and v = R⋅`in` = | 3 |
//      | 1 0 0 |                       | 1 |
//
// `row_indices` need not be in sorted order, and `out` need not be properly
// sized on entry (it will be resized as necessary). However, this method aborts
// if any element of `row_indices` is out of range  *or* if there are any
// duplicated elements in `row_indices`.
template <class T>
void UnrevisedLemkeSolver<T>::SelectSubVector(const VectorX<T>& in,
    const std::vector<int>& row_indices, VectorX<T>* out) {
  DRAKE_ASSERT(ValidateIndices(row_indices, in.size()));

  const int num_rows = row_indices.size();
  out->resize(num_rows);
  for (int i = 0; i < num_rows; i++) {
    DRAKE_ASSERT(row_indices[i] < in.rows());
    (*out)(i) = in(row_indices[i]);
  }
}

// Utility function for copying an r-dimensional column vector v (designated by
// the indices in `row_indices`) into n-dimensional vector `out`. Let R be the
// r × n "row selection" matrix defined in the documentation of
// SelectSubVector(). Then, the result of the operation is:
// `out` = `out` + Rᵀ⋅(`v` - R⋅`out`)
//
// Consider the following example with `v` set to the vector [ 3 1 ]ᵀ,
// `row_indices = { 2, 0}`, and `out` set to the vector [ 4 5 6 ]. This would
// make:
// R =  | 0 0 1 |      and `out` + Rᵀ⋅(`v` - R⋅`out`) = | 1 |
//      | 1 0 0 |                                       | 5 |
//                                                      | 3 |
//
// `row_indices` need not be in sorted order. This method aborts if any element
// of `row_indices` is out of the range of `out`, if `row_indices` is not the
// same size as `v`, or if there are any duplicated elements in `row_indices`.
template <class T>
void UnrevisedLemkeSolver<T>::SetSubVector(const VectorX<T>& v,
    const std::vector<int>& row_indices, VectorX<T>* out) {
  DRAKE_DEMAND(row_indices.size() == static_cast<size_t>(v.size()));
  DRAKE_ASSERT(ValidateIndices(row_indices, out->size()));
  for (size_t i = 0; i < row_indices.size(); ++i)
    (*out)[row_indices[i]] = v[i];
}

// Function for checking whether a set of indices that specify a view into
// a vector is valid. Returns `true` if row_indices are unique and each element
// lies in [0, vector_size-1] and `false` otherwise.
template <class T>
bool UnrevisedLemkeSolver<T>::ValidateIndices(
    const std::vector<int>& row_indices, int vector_size) {
  // Don't check anything for empty vectors.
  if (row_indices.empty())
    return true;

  // Sort the vector first.
  std::vector<int> sorted_row_indices = row_indices;
  std::sort(sorted_row_indices.begin(), sorted_row_indices.end());

  // Validate the maximum and minimum elements.
  if (sorted_row_indices.back() >= vector_size)
    return false;
  if (sorted_row_indices.front() < 0)
    return false;

  // Make sure that the vector is unique.
  return std::unique(sorted_row_indices.begin(), sorted_row_indices.end()) ==
         sorted_row_indices.end();
}

// Function for checking whether a set of indices that specify a view into
// a matrix is valid. Returns `true` if each element of row_indices is unique
// lies in [0, num_rows-1] and if each element of col_indices is unique and
// lies in [0, num_cols-1]. Returns `false` otherwise.
template <class T>
bool UnrevisedLemkeSolver<T>::ValidateIndices(
    const std::vector<int>& row_indices,
    const std::vector<int>& col_indices, int num_rows, int num_cols) {
  return ValidateIndices(row_indices, num_rows) &&
         ValidateIndices(col_indices, num_cols);
}

// Utility function for copying an r × c dimensional submatrix S (designated by
// the indices in row_indices and col_indices) from a matrix M to a target
// matrix, `out`. Let M be the matrix `in` augmented with a single column of
// ones (i.e., the "covering vector"); put another way, M = | in 1 |, where "in"
// refers the n × m-dimensional matrix `in` and 1 is a column of ones (so M is
// n × (m+1)-dimensional). Let R be a r × n "row selection" matrix, constructed
// using `row_indices` and C be a (m+1) × c-dimensional "column selection"
// matrix constructed using `col_indices`. R and C are constructed using
// r = `row_indices.size()` and c = `col_indices.size()` as follows:
//
// Rᵢⱼ = { 1  if j = row_indices[i], ∀i ∈ 0..r-1, ∀j ∈ 0..n-1
//       { 0  otherwise
// Cᵢⱼ = { 1  if col_indices[j] = i, ∀i ∈ 0..m, ∀j ∈ 0..c-1
//       { 0  otherwise
//
// Consider the following example with `in` set to the 3 × 3 identity matrix,
// `row_indices = { 2, 1, 0}` and `col_indices = { 1, 2, 3 }`. This would make:
// R =  | 0 0 1 |      C = | 0 0 0 |   and  R⋅M⋅C = | 0 1 1 |
//      | 0 1 0 |          | 1 0 0 |                | 1 0 1 |
//      | 1 0 0 |          | 0 1 0 |                | 0 0 1 |
//                         | 0 0 1 |
// `row_indices` and `col_indices` need not be in sorted order, and `out` need
// not be properly sized on entry (it will be resized as necessary). However,
// this method aborts if any element of `row_indices` or `col_indices` is out
// of range *or* if there are any duplicated elements.
template <class T>
void UnrevisedLemkeSolver<T>::SelectSubMatrixWithCovering(
    const MatrixX<T>& in,
    const std::vector<int>& row_indices,
    const std::vector<int>& col_indices, MatrixX<T>* out) {
  const int num_rows = row_indices.size();
  const int num_cols = col_indices.size();
  DRAKE_ASSERT(
      ValidateIndices(row_indices, col_indices, in.rows(), in.cols() + 1));
  out->resize(num_rows, num_cols);

  for (int i = 0; i < num_rows; i++) {
    const auto row_in = in.row(row_indices[i]);

    // `row_out` is a "view" into `out`: any modifications to row_out are
    // reflected in `out`.
    auto row_out = out->row(i);
    for (int j = 0; j < num_cols; j++) {
      if (col_indices[j] < in.cols()) {
        DRAKE_ASSERT(col_indices[j] >= 0);
        row_out(j) = row_in(col_indices[j]);
      } else {
        DRAKE_ASSERT(col_indices[j] == in.cols());
        row_out(j) = 1.0;
      }
    }
  }
}

// Determines the various index sets defined in Section 1.1 of [Dai 2018].
template <class T>
void UnrevisedLemkeSolver<T>::DetermineIndexSets() const {
  // Helper for determining index sets.
  auto DetermineIndexSetsHelper = [this](
      const std::vector<LCPVariable>& variables, bool is_z,
      std::vector<int>* variable_set,
      std::vector<int>* variable_set_prime) {
    variable_and_array_indices_.clear();
    for (int i = 0; i < static_cast<int>(variables.size()); ++i) {
      if (variables[i].is_z() == is_z)
        variable_and_array_indices_.emplace_back(variables[i].index(), i);
    }
    std::sort(variable_and_array_indices_.begin(),
              variable_and_array_indices_.end());

    // Construct the set and the primed set.
    for (const auto& variable_and_array_index_pair :
        variable_and_array_indices_) {
      variable_set->push_back(variable_and_array_index_pair.first);
      variable_set_prime->push_back(variable_and_array_index_pair.second);
    }
  };

  // Clear all sets.
  index_sets_.alpha.clear();
  index_sets_.alpha_bar.clear();
  index_sets_.alpha_prime.clear();
  index_sets_.alpha_bar_prime.clear();
  index_sets_.beta.clear();
  index_sets_.beta_bar.clear();
  index_sets_.beta_prime.clear();
  index_sets_.beta_bar_prime.clear();

  DetermineIndexSetsHelper(indep_variables_, false,
      &index_sets_.alpha, &index_sets_.alpha_prime);
  DetermineIndexSetsHelper(dep_variables_, false,
      &index_sets_.alpha_bar, &index_sets_.alpha_bar_prime);
  DetermineIndexSetsHelper(dep_variables_, true,
                           &index_sets_.beta, &index_sets_.beta_prime);
  DetermineIndexSetsHelper(indep_variables_, true,
                           &index_sets_.beta_bar, &index_sets_.beta_bar_prime);
}

// Verifies that each element of the pivoting set is unique. This is an
// expensive operation and should only be executed in Debug mode.
template <class T>
bool UnrevisedLemkeSolver<T>::IsEachUnique(
    const std::vector<LCPVariable>& vars) {
  // Copy the set.
  std::vector<LCPVariable> vars_copy = vars;
  std::sort(vars_copy.begin(), vars_copy.end());
  return (std::unique(vars_copy.begin(), vars_copy.end()) == vars_copy.end());
}

// Performs the pivoting operation, which is described in [Dai 2018].
// `M_prime_col` can be null, if the updated column of M' (pivoted version of M)
// is not needed and the driving_index corresponds to the artificial variable.
template <typename T>
bool UnrevisedLemkeSolver<T>::LemkePivot(
    const MatrixX<T>& M,
    const VectorX<T>& q,
    int driving_index,
    T zero_tol,
    VectorX<T>* M_prime_col,
    VectorX<T>* q_prime) const {
  DRAKE_DEMAND(q_prime);

  const int kArtificial = M.rows();
  DRAKE_DEMAND(driving_index >= 0 && driving_index <= kArtificial);

  // Verify that each member in the independent and dependent sets is unique.
  DRAKE_ASSERT(IsEachUnique(indep_variables_));
  DRAKE_ASSERT(IsEachUnique(dep_variables_));

  // If the driving index does not correspond to the artificial variable,
  // M_prime_col must be non-null.
  if (!IsArtificial(indep_variables_[driving_index]))
    DRAKE_DEMAND(M_prime_col);

  // Determine the sets.
  DetermineIndexSets();

  // Note: It is feasible to do a low-rank update to the factorization below,
  // since alpha and beta should change by no more than a single index between
  // consecutive pivots. Eigen only supports low-rank updates to Cholesky
  // factorizations at the moment, however.

  // Compute matrix and vector views.
  SelectSubMatrixWithCovering(M, index_sets_.alpha, index_sets_.beta,
                              &M_alpha_beta_);
  SelectSubMatrixWithCovering(M, index_sets_.alpha_bar, index_sets_.beta,
                  &M_alpha_bar_beta_);
  SelectSubVector(q, index_sets_.alpha, &q_alpha_);
  SelectSubVector(q, index_sets_.alpha_bar, &q_alpha_bar_);

  // Equation (2) from [Dai 2018].
  // Note: this equation, and those below, must be kept up-to-date with
  // [Dai 2018].
  LinearSolver<T> fMab(M_alpha_beta_);  // Factorized M_alpha_beta_.
  q_prime_beta_prime_ = -fMab.Solve(q_alpha_);

  // Check whether the solution is sufficiently close. We need to do this
  // because partial pivoting LU does not estimate rank (and, from prior
  // experience in solving LCPs), loss of rank need not lead to errors in
  // solving the LCP. We assume that if the factorization is good enough to
  // solve this linear system, it's good enough to solve the subsequent
  // linear system (below). NOTE: current unit tests do not exercise the
  // affirmative evaluation of the conditional (meaning that the "return false"
  // never gets called).
  // @TODO(edrumwri) Institute a unit test that exercises the affirmative
  //   evaluation branch of the conditional when such a LCP has been
  //   identified.
  if ((M_alpha_beta_ * q_prime_beta_prime_ + q_alpha_).norm() > zero_tol)
    return false;

  // Equation (3) from [Dai 2018].
  q_prime_alpha_bar_prime_ = M_alpha_bar_beta_ * q_prime_beta_prime_ +
      q_alpha_bar_;

  // Set the components of q'.
  SetSubVector(q_prime_beta_prime_, index_sets_.beta_prime, q_prime);
  SetSubVector(q_prime_alpha_bar_prime_, index_sets_.alpha_bar_prime, q_prime);

  DRAKE_LOGGER_DEBUG("q': {}", q_prime->transpose());

  // If it is not necessary to compute the column of M, quit now.
  if (!M_prime_col)
    return true;

  // Examine the driving variable.
  if (!indep_variables_[driving_index].is_z()) {
    DRAKE_LOGGER_DEBUG("Driving case #1: driving variable from w");
    // Case from Section 2.2.1.
    // Determine gamma by determining the position of the driving variable
    // in INDEPENDENT W (as defined in [Dai 2018]).
    const int n = static_cast<int>(indep_variables_.size());
    int gamma = 0;
    for (int i = 0; i < n; ++i) {
      if (!indep_variables_[i].is_z()) {
        if (indep_variables_[i].index() <
            indep_variables_[driving_index].index()) {
          ++gamma;
        }
      }
    }

    // From Equation (4) in [Dai 2018].
    DRAKE_ASSERT(index_sets_.alpha[gamma] ==
        indep_variables_[driving_index].index());

    // Set the unit vector.
    e_.setZero(index_sets_.beta.size());
    e_[gamma] = 1.0;

    // Equation (5).
    M_prime_driving_beta_prime_ = fMab.Solve(e_);

    // Equation (6).
    M_prime_driving_alpha_bar_prime_ = M_alpha_bar_beta_ *
        M_prime_driving_beta_prime_;
  } else {
    DRAKE_LOGGER_DEBUG("Driving case #2: driving variable from z");

    // Case from Section 2.2.2 of [Dai 2018].
    // Determine zeta.
    const int zeta = indep_variables_[driving_index].index();

    // Compute g_alpha and g_alpha_bar.
    SelectSubColumnWithCovering(M, index_sets_.alpha, zeta, &g_alpha_);
    SelectSubColumnWithCovering(M, index_sets_.alpha_bar, zeta, &g_alpha_bar_);

    // Equation (7).
    M_prime_driving_beta_prime_ = -fMab.Solve(g_alpha_);

    // Equation (8).
    M_prime_driving_alpha_bar_prime_ = g_alpha_bar_ +
        M_alpha_bar_beta_ * M_prime_driving_beta_prime_;
  }

  SetSubVector(M_prime_driving_beta_prime_, index_sets_.beta_prime,
               M_prime_col);
  SetSubVector(M_prime_driving_alpha_bar_prime_, index_sets_.alpha_bar_prime,
               M_prime_col);

  DRAKE_LOGGER_DEBUG("M' (driving): {}", M_prime_col->transpose());
  return true;
}

// Checks to see whether a given variable is the artificial variable.
template <class T>
bool UnrevisedLemkeSolver<T>::IsArtificial(const LCPVariable& v) const {
  const int n = static_cast<int>(dep_variables_.size());
  return v.is_z() && v.index() == n;
}

// Method for finding the index of the complement of an LCP variable in
// a tuple (strictly speaking, an unsorted vector) of independent variables.
// Aborts if the index is not found in the set or the variable is the artificial
// variable (it never should be).
template <class T>
int UnrevisedLemkeSolver<T>::FindComplementIndex(
    const LCPVariable& query) const {
  // Verify that the query is not the artificial variable.
  DRAKE_DEMAND(!IsArtificial(query));

  const auto iter = indep_variables_indices_.find(query.Complement());
  DRAKE_DEMAND(iter != indep_variables_indices_.end());
  return iter->second;
}

// Computes the solution using the current index sets. `z` must simply be
// non-null; it will be resized as necessary. Returns `true` if able to
// construct the solution and `false` if unable to find the solution to a
// necessary system of linear equations. Aborts (in LemkePivot()) if
// `artificial_index` does not correspond to the index of the artificial
// variable in the vector of independent_variables.
// @pre The artificial variable was the blocking variable, indicating that the
//      solution to the LCP can be obtained after a final pivoting operation.
// @pre `artificial_index` corresponds to the index of the artificial variable
//      in the vector of independent variables.
template <class T>
bool UnrevisedLemkeSolver<T>::ConstructLemkeSolution(
    const MatrixX<T>& M,
    const VectorX<T>& q,
    int artificial_index,
    T zero_tol,
    VectorX<T>* z) const {
  DRAKE_DEMAND(z);
  const int n = q.rows();

  // Compute the solution by pivoting the artificial variable, which was just
  // identified as the blocking variable, from the set of dependent variables
  // to the set of independent variables.
  VectorX<T> q_prime(n);
  if (!LemkePivot(M, q, artificial_index, zero_tol, nullptr, &q_prime))
    return false;

  z->setZero(n);
  for (int i = 0; i < static_cast<int>(dep_variables_.size()); ++i) {
    if (dep_variables_[i].is_z())
      (*z)[dep_variables_[i].index()] = q_prime[i];
  }
  return true;
}

// Computes the blocking index using the minimum ratio test. Returns `true`
// if successful, `false` if not (due to, e.g., the driving variable being
// "unblocked" or a cycle being detected). If `false`, `blocking_index` will
// set to -1 on return.
template <typename T>
bool UnrevisedLemkeSolver<T>::FindBlockingIndex(
    const T& zero_tol, const VectorX<T>& matrix_col, const VectorX<T>& ratios,
    int* blocking_index) const {
  DRAKE_DEMAND(blocking_index);
  DRAKE_DEMAND(ratios.size() == matrix_col.size());
  DRAKE_DEMAND(zero_tol > 0);

  const int n = matrix_col.size();
  T min_ratio = std::numeric_limits<double>::infinity();
  *blocking_index = -1;
  for (int i = 0; i < n; ++i) {
    if (matrix_col[i] < -zero_tol) {
      DRAKE_LOGGER_DEBUG("Ratio for index {}: {}", i, ratios[i]);
      if (ratios[i] < min_ratio) {
        min_ratio = ratios[i];
        *blocking_index = i;
      }
    }
  }

  if (*blocking_index < 0) {
    DRAKE_LOGGER_DEBUG("driving variable is unblocked- algorithm failed");
    return false;
  }

  // Determine all variables within the zero tolerance of the minimum ratio,
  // while simultaneously looking for the presence of the artificial variable
  // among the (possible multiple) minima.
  std::vector<int> blocking_indices;
  for (int i = 0; i < n; ++i) {
    if (matrix_col[i] < -zero_tol) {
      DRAKE_LOGGER_DEBUG("Ratio for index {}: {}", i, ratios[i]);
      if (ratios[i] < min_ratio + zero_tol) {
        if (IsArtificial(dep_variables_[i])) {
          // *Always* select the artificial variable, if multiple choices are
          // possible ([Cottle 1992] p. 280).
          *blocking_index = i;
          return true;
        }
        blocking_indices.push_back(i);
      }
    }
  }

  // If there are multiple blocking variables, replace the blocking index with
  // the cycling selection.
  if (blocking_indices.size() > 1) {
    auto& index = selections_[indep_variables_];

    // Verify that we have not run out of indices to select, which means that
    // cycling would be occurring, in spite of cycling prevention.
    if (index >= static_cast<int>(blocking_indices.size())) {
      DRAKE_LOGGER_DEBUG("Cycling detected- indicating failure.");
      *blocking_index = -1;
      return false;
    }
    *blocking_index = blocking_indices[index];
    ++index;
  }

  return true;
}

template <typename T>
bool UnrevisedLemkeSolver<T>::IsSolution(
    const MatrixX<T>& M, const VectorX<T>& q, const VectorX<T>& z,
    T zero_tol) {
  using std::abs;

  const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M);

  // Find the minima of z and w.
  const T min_z = z.minCoeff();
  const auto w = M * z + q;
  const T min_w = w.minCoeff();

  // Compute the dot product of z and w.
  const T dot = w.dot(z);
  const int n = q.size();
  return (min_z > -mod_zero_tol && min_w > -mod_zero_tol &&
          abs(dot) < 10 * n * mod_zero_tol);
}

// Note: maintainers should read Section 4.4 - 4.4.5 of [Cottle 1992] to
// understand Lemke's Algorithm (and this function).
template <typename T>
bool UnrevisedLemkeSolver<T>::SolveLcpLemke(const MatrixX<T>& M,
                                     const VectorX<T>& q, VectorX<T>* z,
                                     int* num_pivots,
                                     const T& zero_tol) const {
  using std::max;
  using std::abs;
  DRAKE_DEMAND(num_pivots);

  DRAKE_LOGGER_DEBUG(
      "UnrevisedLemkeSolver::SolveLcpLemke() entered, M: {}, "
      "q: {}, ", M, q.transpose());

  const int n = q.size();
  const int max_pivots = 50 * n;  // O(n) pivots expected for solvable problems.

  if (M.rows() != n || M.cols() != n)
    throw std::logic_error("M's dimensions do not match that of q.");

  // Update the pivots.
  *num_pivots = 0;

  // Look for immediate exit.
  if (n == 0) {
    DRAKE_LOGGER_DEBUG("-- LCP is zero dimensional");
    z->resize(0);
    return true;
  }

  // Denote the index of the artificial variable (i.e., the variable denoted
  // z₀ in [Cottle 1992], p. 266). Because z₀ is prone to being confused with
  // the first dimension of z in 0-indexed languages like C++, we refer to the
  // artificial variable as zₙ in this implementation (it is denoted zₙ₊₁ in
  // [Dai 2018], as that document uses the 1-indexing prevalent in algorithmic
  // descriptions).
  const int kArtificial = n;

  // Compute a sensible value for zero tolerance if none is given.
  T mod_zero_tol = zero_tol;
  if (mod_zero_tol <= 0)
    mod_zero_tol = ComputeZeroTolerance(M);

  // Checks to see whether the trivial solution z = 0 to the LCP w = Mz + q
  // solves the LCP. This must be the case if q is non-negative, as w would then
  // be non-negative, z would be non-negative (zero), and w'z = 0.
  if (q.minCoeff() > -mod_zero_tol) {
    z->setZero(q.size());
    DRAKE_LOGGER_DEBUG(" -- trivial solution found");
    DRAKE_LOGGER_DEBUG("UnrevisedLemkeSolver::SolveLcpLemke() exited");
    return true;
  }

  // Clear the cycling selections.
  selections_.clear();

  // If 'n' is identical to the size of the last problem solved, try using the
  // indices from the last problem solved.
  if (static_cast<size_t>(n) == dep_variables_.size()) {
    // Verify that the last call found a solution (indicated by the presence
    // of the artificial variable (zn) in the independent set).
    int zn_index = -1;
    for (int i = 0;
         i < static_cast<int>(indep_variables_.size()) && zn_index < 0; ++i) {
      if (IsArtificial(indep_variables_[i]))
        zn_index = i;
    }

    if (zn_index >= 0) {
      // Compute the candidate solution.
      if (ConstructLemkeSolution(M, q, zn_index, mod_zero_tol, z)) {
        if (IsSolution(M, q, *z, mod_zero_tol)) {
          // If z truly is the solution, return now, indicating only one pivot
          // (in the solution construction) was performed.
          ++(*num_pivots);
          return true;
        }
      } else {
        DRAKE_LOGGER_DEBUG(
            "Failed to solve linear system implied by last solution");
      }
    }
  }

  // Set the LCP variables. Start with all z variables independent and all w
  // variables dependent.
  indep_variables_.resize(n+1);
  dep_variables_.resize(n);
  for (int i = 0; i < n; ++i) {
    dep_variables_[i] = LCPVariable(false, i);
    indep_variables_[i] = LCPVariable(true, i);
  }
  // z needs one more variable (the artificial variable), whose index we
  // denote as n to keep it from corresponding to any actual vector index.
  indep_variables_[n] = LCPVariable(true, n);

  // Compute zn*, the smallest value of the artificial variable zn for which
  // w = q + zn >= 0. Let blocking denote a component of w that equals
  // zero when zn = zn*.
  int blocking_index = -1;
  bool blocking_index_found = FindBlockingIndex(
      mod_zero_tol, q, q, &blocking_index);
  DRAKE_DEMAND(blocking_index_found);

  // Pivot blocking, artificial. Note that we rely upon the dependent variables
  // being ordered sequentially in both arrays.
  LCPVariable blocking = dep_variables_[blocking_index];
  int driving_index = blocking.index();
  std::swap(dep_variables_[blocking_index], indep_variables_[kArtificial]);
  DRAKE_LOGGER_DEBUG("First blocking variable {}{}",
                     ((blocking.is_z()) ? "z" : "w"), blocking.index());
  DRAKE_LOGGER_DEBUG("First driving variable (artificial)");

  // Initialize the independent variable indices. We do this after the initial
  // variable swap for simplicity.
  for (int i = 0; i < static_cast<int>(indep_variables_.size()); ++i)
    indep_variables_indices_[indep_variables_[i]] = i;

  // Output the independent and dependent variable tuples.
  auto to_string = [](const std::vector<LCPVariable>& vars) -> std::string {
    std::ostringstream oss;
    for (int i = 0; i < static_cast<int>(vars.size()); ++i)
      oss << ((vars[i].is_z()) ? "z" : "w") << vars[i].index() << " ";
    return oss.str();
  };
  unused(to_string);  // ... when in release mode.
  DRAKE_LOGGER_DEBUG("Independent set variables: {}",
      to_string(indep_variables_));
  DRAKE_LOGGER_DEBUG("Dependent set variables: {}",
      to_string(dep_variables_));

  // Pivot up to the maximum number of times.
  VectorX<T> q_prime(n), M_prime_col(n);
  while (++(*num_pivots) < max_pivots) {
    DRAKE_LOGGER_DEBUG("New driving variable {}{}",
                       ((indep_variables_[driving_index].is_z()) ? "z" : "w"),
                       indep_variables_[driving_index].index());

    // Compute the permuted q and driving column of the permuted M matrix.
    if (!LemkePivot(
        M, q, driving_index, mod_zero_tol, &M_prime_col, &q_prime)) {
      DRAKE_LOGGER_DEBUG("Linear system solve failed.");
      z->setZero(n);
      return false;
    }

    // Find the blocking variable.
    if (!FindBlockingIndex(
        mod_zero_tol, M_prime_col,
        -(q_prime.array() / M_prime_col.array()).matrix(), &blocking_index)) {
      z->setZero(n);
      return false;
    }
    blocking = dep_variables_[blocking_index];
    DRAKE_LOGGER_DEBUG("Blocking variable {}{}",
                       ((blocking.is_z()) ? "z" : "w"), blocking.index());

    // See whether the artificial variable blocks the driving variable.
    if (blocking.index() == kArtificial) {
      DRAKE_DEMAND(blocking.is_z());

      // Pivot zn with the driving variable.
      std::swap(dep_variables_[blocking_index],
                indep_variables_[driving_index]);

      // Compute the permuted q, and convert it into a solution.
      if (ConstructLemkeSolution(M, q, driving_index, mod_zero_tol, z)) {
        if (IsSolution(M, q, *z))
          return true;

        DRAKE_LOGGER_DEBUG("Solution not computed to requested tolerance");
        z->setZero(n);
        return false;
      }

      // Otherwise, indicate failure.
      DRAKE_LOGGER_DEBUG(
          "Linear system solver failed to construct Lemke solution");
      z->setZero(n);
      return false;
    }

    // Pivot the blocking variable and the driving variable.
    std::swap(dep_variables_[blocking_index],
              indep_variables_[driving_index]);

    // Update the index map.
    auto indep_variables_indices_iter = indep_variables_indices_.find(
        dep_variables_[blocking_index]);
    indep_variables_indices_.erase(indep_variables_indices_iter);
    indep_variables_indices_[indep_variables_[driving_index]] =
        driving_index;

    // Make the driving variable the complement of the blocking variable.
    driving_index = FindComplementIndex(blocking);

    DRAKE_LOGGER_DEBUG("Independent set variables: {}",
        to_string(indep_variables_));
    DRAKE_LOGGER_DEBUG("Dependent set variables: {}",
        to_string(dep_variables_));
  }

  // If here, the maximum number of pivots has been exceeded.
  z->setZero(n);
  DRAKE_LOGGER_DEBUG("Maximum number of pivots exceeded");
  return false;
}

template <typename T>
UnrevisedLemkeSolver<T>::UnrevisedLemkeSolver()
    : SolverBase(&id, &is_available, &is_enabled,
                 &ProgramAttributesSatisfied) {}

template <typename T>
UnrevisedLemkeSolver<T>::~UnrevisedLemkeSolver() = default;

SolverId UnrevisedLemkeSolverId::id() {
  static const never_destroyed<SolverId> singleton{"Unrevised Lemke"};
  return singleton.access();
}

template <typename T>
SolverId UnrevisedLemkeSolver<T>::id() {
  return UnrevisedLemkeSolverId::id();
}

template <typename T>
bool UnrevisedLemkeSolver<T>::is_available() {
  return true;
}

template <typename T>
bool UnrevisedLemkeSolver<T>::is_enabled() {
  return true;
}

template <typename T>
bool UnrevisedLemkeSolver<T>::ProgramAttributesSatisfied(
    const MathematicalProgram& prog) {
  // This solver imposes restrictions that its problem:
  //
  // (1) Contains only linear complementarity constraints,
  // (2) Has no element of any decision variable appear in more than one
  //     constraint, and
  // (3) Has every element of every decision variable in a constraint.
  //
  // Restriction 1 could reasonably be relaxed by reformulating other
  // constraint types that can be expressed as LCPs (eg, convex QLPs),
  // although this would also entail adding an output stage to convert
  // the LCP results back to the desired form.  See eg. @RussTedrake on
  // how to convert a linear equality constraint of n elements to an
  // LCP of 2n elements.
  //
  // There is no obvious way to relax restriction 2.
  //
  // Restriction 3 could reasonably be relaxed to simply let unbound
  // variables sit at 0.
  if (prog.required_capabilities() != ProgramAttributes({
        ProgramAttribute::kLinearComplementarityConstraint})) {
    return false;
  }

  // Check that the available LCPs cover the program and no two LCPs cover the
  // same variable.
  const auto& bindings = prog.linear_complementarity_constraints();
  for (int i = 0; i < static_cast<int>(prog.num_vars()); ++i) {
    int coverings = 0;
    for (const auto& binding : bindings) {
      if (binding.ContainsVariable(prog.decision_variable(i))) {
        coverings++;
      }
    }
    if (coverings != 1) {
      return false;
    }
  }

  return true;
}

}  // namespace solvers
}  // namespace drake

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
    class ::drake::solvers::UnrevisedLemkeSolver)


//
// Created by AIYOJ on 2019-03-02.
//

#include "log/algocomp_log.h"
#include "calibration/isotonic_regression.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace algocomp {
    IsotonicRegression::IsotonicRegression() {
        _logger = spdlog::get(ACLog::_logger_name);
        _logger->info("Initialize Isotonic Regression.");
    }

    IsotonicRegression::~IsotonicRegression() {
        _logger->info("Leave Isotonic Regression.");
    }

    std::vector<float> IsotonicRegression::calibrate(const std::vector<float> &scores) {
        std::vector<float> calibrated_scores;
        for (int i = 0; i < scores.size(); ++i) {
            auto calibrated_score = get(scores[i], _search_type);
            calibrated_scores.push_back(calibrated_score);
        }

        return calibrated_scores;
    }

    float IsotonicRegression::calibrate(float score) {
        float calibrated_score = get(score, _search_type);
        return calibrated_score;
    }

    std::unique_ptr<IsotonicRegression> IsotonicRegression::load(const std::string &path) {
        auto logger = spdlog::get(ACLog::_logger_name);

        try {
            std::ifstream file;
            file.open(path);
            if (file.is_open()) {
                std::unique_ptr<IsotonicRegression> output_calibration = std::unique_ptr<IsotonicRegression>(
                        new IsotonicRegression());
                std::string line;
                float pre = 0;
                bool order = true;
                std::vector<std::pair<float, float >> tmp_bins;
                while (!file.eof()) {
                    std::getline(file, line);
                    line = boost::algorithm::trim_copy(line);
                    if (line.length() == 0) {
                        continue;
                    }

                    std::vector<std::string> splits;
                    boost::algorithm::split(splits, line, boost::algorithm::is_any_of(" "));

                    if (splits.size() != 2) {
                        logger->error("Invalid isotonic regression coef {}", line);
                        continue;
                    }

                    float bin = boost::lexical_cast<float>(splits[0]);
                    float value = boost::lexical_cast<float>(splits[1]);

                    if (bin > pre) {
                        order = true;
                        pre = bin;
                    } else if (bin < pre) {
                        order = false;
                        pre = bin;
                    } else {
                        logger->error("the same bin: {}", bin);
                        break;
                    }

                    tmp_bins.push_back(std::make_pair(bin, value));
                }

                if (order) {
                    output_calibration->update(tmp_bins);
                } else {
                    // sort
                    std::sort(tmp_bins.begin(), tmp_bins.end(),
                              [](const std::pair<float, float> &a, const std::pair<float, float> &b) {
                                  return a.first < b.first;
                              });
                    output_calibration->update(tmp_bins);
                }
                if (file.is_open()) {
                    file.close();
                }
                if (output_calibration) {
                    logger->info("Natively loaded isotonic regression from {}.", path);
                    return output_calibration;
                } else {
                    logger->error("Fail to read calibration file when natively loading isotonic regression from {}.",
                                  path);
                }
            } else {
                logger->error(
                        "Fail to open isotonic regression file when natively loading isotonic regression from {}.",
                        path);
            }

        } catch (std::exception &ex) {
            logger->error("Fail to natively load isotonic regression from {}, detail: {}.", path, ex.what());
        }
        return nullptr;
    }

    void IsotonicRegression::update(const std::vector<std::pair<float, float>> &bins) {
        _bins.clear();
        _bins = bins;
    }

    float IsotonicRegression::get(float bin, const SearchType search_type) {
        // upper bound
        auto upper_bound = [this](float key) {
            int first = 0;
            int len = _bins.size() - 1;
            int half, middle;

            while (len > 0) {
                half = len >> 1;
                middle = first + half;
                // The median is greater than the key and is looked up in the left half of the sequence containing last.
                if (_bins[middle].first > key) {
                    len = half;
                } else {  // The median is less than or equal to the key and is looked up in the right half of the sequence.
                    first = middle + 1;
                    len = len - half - 1;
                }
            }
            return first;
        };

        // lower bound
        auto lower_bound = [this](float key) {
            int first = 0, middle;
            int half, len;
            len = _bins.size();

            while (len > 0) {
                half = len >> 1;
                middle = first + half;
                if (_bins[middle].first < key) {
                    first = middle + 1;
                    len = len - half - 1;
                } else {
                    len = half;
                }
            }
            return first;
        };

        // search type
        if (search_type == SearchType::UPPERBOUND) {
            auto index = upper_bound(bin);
            return _bins[index].second;
        } else if (search_type == SearchType::LOWERBOUND) {
            auto index = lower_bound(bin);
            return _bins[index].second;
        } else {
            _logger->error("Unknown Search Type.");
        }
    }
}


/**
 * @file cape.cpp
 *
 */

#include "cape.h"
#include "logger.h"
#include "numerical_functions.h"
#include "plugin_factory.h"
#include "util.h"
#include <boost/lexical_cast.hpp>
#include <boost/thread.hpp>
#include <future>

#include "fetcher.h"
#include "hitool.h"
#include "radon.h"

#include "cape.cuh"

const unsigned char FCAPE = (1 << 2);
const unsigned char FCAPE3km = (1 << 0);

using namespace std;
using namespace himan::plugin;

#ifdef DEBUG
#define DumpVector(A, B) himan::util::DumpVector(A, B)
#else
#define DumpVector(A, B)
#endif

extern mutex dimensionMutex;

const himan::param LCLTParam("LCL-K", 4, 0, 0, 0);
const himan::param LCLPParam("LCL-HPA", 4720, 0, 3, 0);
const himan::param LCLZParam("LCL-M", 4726, 0, 3, 6);
const himan::param LFCTParam("LFC-K", 4, 0, 0, 0);
const himan::param LFCPParam("LFC-HPA", 4721, 0, 3, 0);
const himan::param LFCZParam("LFC-M", 4727, 0, 3, 6);
const himan::param ELTParam("EL-K", 4, 0, 0, 0);
const himan::param ELPParam("EL-HPA", 4722, 0, 3, 0);
const himan::param ELZParam("EL-M", 4728, 0, 3, 6);
const himan::param CAPEParam("CAPE-JKG", 4723, 0, 7, 6);
const himan::param CAPE1040Param("CAPE1040-JKG", 4729, 0, 7, 6);
const himan::param CAPE3kmParam("CAPE3KM-JKG", 4724, 0, 7, 6);
const himan::param CINParam("CIN-JKG", 4725, 0, 7, 7);

const himan::level SURFACE(himan::kHeight, 0);
const himan::level M500(himan::kHeightLayer, 500, 0);
const himan::level UNSTABLE(himan::kMaximumThetaE, 0);

double Max(const vector<double>& vec)
{
	double ret = -1e38;

	for (const double& val : vec)
	{
		if (val != himan::kFloatMissing && val > ret) ret = val;
	}

	if (ret == -1e38) ret = himan::kFloatMissing;

	return ret;
}

void MultiplyWith(vector<double>& vec, double multiplier)
{
	for (double& val : vec)
	{
		if (val != himan::kFloatMissing) val *= multiplier;
	}
}

string PrintMean(const vector<double>& vec)
{
	double min = 1e38, max = -1e38, sum = 0;
	size_t count = 0, missing = 0;

	for (const double& val : vec)
	{
		if (val == himan::kFloatMissing)
		{
			missing++;
			continue;
		}

		min = (val < min) ? val : min;
		max = (val > max) ? val : max;
		count++;
		sum += val;
	}

	double mean = numeric_limits<double>::quiet_NaN();

	if (count > 0)
	{
		mean = sum / static_cast<double>(count);
	}

	string minstr = (min == 1e38) ? "nan" : to_string(static_cast<int>(min));
	string maxstr = (max == -1e38) ? "nan" : to_string(static_cast<int>(max));
	string meanstr = (mean != mean) ? "nan" : to_string(static_cast<int>(mean));

	return "min " + minstr + " max " + maxstr + " mean " + meanstr + " missing " + to_string(missing);
}

void MoistLift(const double* Piter, const double* Titer, const double* Penv, double* Tparcel, size_t size)
{
	// Split MoistLift (integration of a saturated air parcel upwards in atmosphere)
	// to several threads since it is very CPU intensive

	vector<future<void>> futures;

	size_t workers = 6;

	if (size % workers != 0)
	{
		workers = 4;
		if (size % workers != 0)
		{
			workers = 3;
			if (size % workers != 0)
			{
				workers = 1;
			}
		}
	}

	const size_t splitSize = static_cast<size_t>(floor(size / workers));

	for (size_t num = 0; num < workers; num++)
	{
		const size_t start = num * splitSize;
		futures.push_back(async(launch::async,
		                        [&](size_t start) {
			                        himan::metutil::MoistLiftA(&Piter[start], &Titer[start], &Penv[start],
			                                                   &Tparcel[start], splitSize);
			                    },
		                        start));
	}

	for (auto& future : futures)
	{
		future.get();
	}
}

cape::cape() : itsBottomLevel(kHybrid, kHPMissingInt)
{
	itsLogger = logger("cape");
}

void cape::Process(std::shared_ptr<const plugin_configuration> conf)
{
	compiled_plugin_base::Init(conf);

	auto r = GET_PLUGIN(radon);

	itsBottomLevel = level(kHybrid, stoi(r->RadonDB().GetProducerMetaData(itsConfiguration->SourceProducer().Id(),
	                                                                      "last hybrid level number")));

#ifdef HAVE_CUDA
	cape_cuda::itsBottomLevel = itsBottomLevel;
#endif

	vector<param> theParams;
	vector<string> sourceDatas;

	if (itsConfiguration->Exists("source_data"))
	{
		sourceDatas = itsConfiguration->GetValueList("source_data");
	}

	if (sourceDatas.size() == 0)
	{
		sourceDatas.push_back("surface");
		sourceDatas.push_back("500m mix");
		sourceDatas.push_back("most unstable");
	}

	theParams.push_back(LCLTParam);
	theParams.push_back(LCLPParam);
	theParams.push_back(LCLZParam);
	theParams.push_back(LFCTParam);
	theParams.push_back(LFCPParam);
	theParams.push_back(LFCZParam);
	theParams.push_back(ELTParam);
	theParams.push_back(ELPParam);
	theParams.push_back(ELZParam);
	theParams.push_back(CAPEParam);
	theParams.push_back(CAPE1040Param);
	theParams.push_back(CAPE3kmParam);
	theParams.push_back(CINParam);

	for (const auto& source : sourceDatas)
	{
		if (source == "surface")
		{
			itsSourceLevels.push_back(SURFACE);
		}
		else if (source == "500m mix")
		{
			itsSourceLevels.push_back(M500);
		}
		else if (source == "most unstable")
		{
			itsSourceLevels.push_back(UNSTABLE);
		}
	}

	// disregard the level information provided by user

	itsConfiguration->Info()->Levels(itsSourceLevels);

	SetParams(theParams);

	Start();
}

void cape::Calculate(shared_ptr<info> myTargetInfo, unsigned short threadIndex)
{
	/*
	 * Algorithm:
	 *
	 * 1) Find suitable T and TD values of an air parcel
	 *
	 * 2) Lift this air parcel to LCL height dry adiabatically
	 *
	 * 3) Continue lifting the particle to LFC height moist adiabatically.
	 *
	 * This is done by lifting the parcel a certain height at a time (500 Pa),
	 * and calculating the new temperature from the lifted height. If parcel
	 * temperature is larger than environment temperature, we have reached LFC.
	 * Environment temperature is therefore fetched every iteration of integration
	 * algorithm.
	 *
	 * 4) Integrate from LFC to EL
	 *
	 * 5) Integrate from surface to LFC to find CIN
	 */

	auto sourceLevel = myTargetInfo->Level();

	auto mySubThreadedLogger = logger("siThread#" + boost::lexical_cast<string>(threadIndex) + "Version" +
	                                  boost::lexical_cast<string>(static_cast<int>(sourceLevel.Type())));

	mySubThreadedLogger.Info("Calculating source level type " + HPLevelTypeToString.at(sourceLevel.Type()) +
	                         " for time " + static_cast<string>(myTargetInfo->Time().ValidDateTime()));

	// 1.

	timer aTimer;
	aTimer.Start();

	cape_source sourceValues;

	switch (sourceLevel.Type())
	{
		case kHeight:
			sourceValues = GetSurfaceValues(myTargetInfo);
			break;

		case kHeightLayer:
			sourceValues = Get500mMixingRatioValues(myTargetInfo);
			break;

		case kMaximumThetaE:
			sourceValues = GetHighestThetaEValues(myTargetInfo);
			break;

		default:
			throw runtime_error("Invalid source level: " + static_cast<std::string>(sourceLevel));
			break;
	}

	myTargetInfo->Level(sourceLevel);

	if (get<0>(sourceValues).empty()) return;

	aTimer.Stop();

	mySubThreadedLogger.Info("Source data calculated in " + boost::lexical_cast<string>(aTimer.GetTime()) + " ms");

	mySubThreadedLogger.Debug("Source temperature: " + ::PrintMean(get<0>(sourceValues)));
	mySubThreadedLogger.Debug("Source dewpoint: " + ::PrintMean(get<1>(sourceValues)));
	mySubThreadedLogger.Debug("Source pressure: " + ::PrintMean(get<2>(sourceValues)));

	// 2.

	aTimer.Start();

	auto LCL = GetLCL(myTargetInfo, sourceValues);

	aTimer.Stop();

	mySubThreadedLogger.Info("LCL calculated in " + boost::lexical_cast<string>(aTimer.GetTime()) + " ms");

	mySubThreadedLogger.Debug("LCL temperature: " + ::PrintMean(LCL.first));
	mySubThreadedLogger.Debug("LCL pressure: " + ::PrintMean(LCL.second));

	myTargetInfo->Param(LCLTParam);
	myTargetInfo->Data().Set(LCL.first);

	myTargetInfo->Param(LCLPParam);
	myTargetInfo->Data().Set(LCL.second);

	auto h = GET_PLUGIN(hitool);
	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());
	h->HeightUnit(kHPa);

	auto height = h->VerticalValue(param("HL-M"), LCL.second);

	myTargetInfo->Param(LCLZParam);
	myTargetInfo->Data().Set(height);

	// 3.

	aTimer.Start();

	auto LFC = GetLFC(myTargetInfo, LCL.first, LCL.second);

	aTimer.Stop();

	mySubThreadedLogger.Info("LFC calculated in " + boost::lexical_cast<string>(aTimer.GetTime()) + " ms");

	if (LFC.first.empty())
	{
		return;
	}

	mySubThreadedLogger.Debug("LFC temperature: " + ::PrintMean(LFC.first));
	mySubThreadedLogger.Debug("LFC pressure: " + ::PrintMean(LFC.second));

	myTargetInfo->Param(LFCTParam);
	myTargetInfo->Data().Set(LFC.first);

	myTargetInfo->Param(LFCPParam);
	myTargetInfo->Data().Set(LFC.second);

	height = h->VerticalValue(param("HL-M"), LFC.second);

	myTargetInfo->Param(LFCZParam);
	myTargetInfo->Data().Set(height);

	// 4. & 5.

	aTimer.Start();

	auto capeInfo = make_shared<info>(*myTargetInfo);
	boost::thread t1(&cape::GetCAPE, this, boost::ref(capeInfo), LFC, ELTParam, ELPParam, ELZParam, CAPEParam,
	                 CAPE1040Param, CAPE3kmParam);

	auto cinInfo = make_shared<info>(*myTargetInfo);
	boost::thread t2(&cape::GetCIN, this, boost::ref(cinInfo), get<0>(sourceValues), get<2>(sourceValues), LCL.first,
	                 LCL.second, LFC.second, CINParam);

	t1.join();
	t2.join();

	aTimer.Stop();

	mySubThreadedLogger.Info("CAPE and CIN calculated in " + boost::lexical_cast<string>(aTimer.GetTime()) + " ms");

	// Sometimes CAPE area is infinitely small -- so that CAPE is zero but LFC is found. In this case set all derivative
	// parameters missing.

	capeInfo->Param(LFCZParam);
	auto& lfcz_ = VEC(capeInfo);
	capeInfo->Param(LFCPParam);
	auto& lfcp_ = VEC(capeInfo);
	capeInfo->Param(LFCTParam);
	auto& lfct_ = VEC(capeInfo);
	cinInfo->Param(CINParam);
	auto& cin_ = VEC(cinInfo);
	capeInfo->Param(ELZParam);
	auto elz_ = VEC(capeInfo);
	capeInfo->Param(CAPEParam);
	auto cape_ = VEC(capeInfo);

	for (size_t i = 0; i < lfcz_.size(); i++)
	{
		if (cape_[i] == 0 && elz_[i] == kFloatMissing && lfcz_[i] != kFloatMissing)
		{
			cin_[i] = 0;
			lfcz_[i] = kFloatMissing;
			lfcp_[i] = kFloatMissing;
			lfct_[i] = kFloatMissing;
		}
	}

#ifdef DEBUG
	assert(lfcz_.size() == elz_.size());
	assert(cape_.size() == elz_.size());
	assert(cin_.size() == elz_.size());

	for (size_t i = 0; i < lfcz_.size(); i++)
	{
		// Check:
		// * If LFC is missing, EL is missing
		// * If LFC is present, EL is present
		// * If both are present, LFC must be below EL
		// * CAPE must be zero or positive real value
		// * CIN must be zero or negative real value
		assert((lfcz_[i] == kFloatMissing && elz_[i] == kFloatMissing) ||
		       ((lfcz_[i] != kFloatMissing && elz_[i] != kFloatMissing) && (lfcz_[i] < elz_[i])));
		assert(cape_[i] >= 0);
		assert(cin_[i] <= 0);
	}
#endif

	// Do smoothening for CAPE & CIN parameters
	mySubThreadedLogger.Trace("Smoothening");

	himan::matrix<double> filter_kernel(3, 3, 1, kFloatMissing, 1. / 9.);

	capeInfo->Param(CAPEParam);
	himan::matrix<double> filtered = numerical_functions::Filter2D(capeInfo->Data(), filter_kernel);
	capeInfo->Grid()->Data(filtered);

	capeInfo->Param(CAPE1040Param);
	filtered = numerical_functions::Filter2D(capeInfo->Data(), filter_kernel);
	capeInfo->Grid()->Data(filtered);

	capeInfo->Param(CAPE3kmParam);
	filtered = numerical_functions::Filter2D(capeInfo->Data(), filter_kernel);
	capeInfo->Grid()->Data(filtered);

	capeInfo->Param(CINParam);
	filtered = numerical_functions::Filter2D(capeInfo->Data(), filter_kernel);
	capeInfo->Grid()->Data(filtered);

	capeInfo->Param(CAPEParam);
	mySubThreadedLogger.Debug("CAPE: " + ::PrintMean(VEC(capeInfo)));
	capeInfo->Param(CAPE1040Param);
	mySubThreadedLogger.Debug("CAPE1040: " + ::PrintMean(VEC(capeInfo)));
	capeInfo->Param(CAPE3kmParam);
	mySubThreadedLogger.Debug("CAPE3km: " + ::PrintMean(VEC(capeInfo)));
	cinInfo->Param(CINParam);
	mySubThreadedLogger.Debug("CIN: " + ::PrintMean(VEC(cinInfo)));
}

void cape::GetCIN(shared_ptr<info> myTargetInfo, const vector<double>& Tsource, const vector<double>& Psource,
                  const vector<double>& TLCL, const vector<double>& PLCL, const vector<double>& PLFC, param CINParam)
{
#ifdef HAVE_CUDA
	if (itsConfiguration->UseCuda())
	{
		cape_cuda::GetCINGPU(itsConfiguration, myTargetInfo, Tsource, Psource, TLCL, PLCL, PLFC, CINParam);
	}
	else
#endif
	{
		GetCINCPU(myTargetInfo, Tsource, Psource, TLCL, PLCL, PLFC, CINParam);
	}
}

void cape::GetCINCPU(shared_ptr<info> myTargetInfo, const vector<double>& Tsource, const vector<double>& Psource,
                     const vector<double>& TLCL, const vector<double>& PLCL, const vector<double>& PLFC, param CINParam)
{
	auto h = GET_PLUGIN(hitool);
	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());
	h->HeightUnit(kHPa);

	vector<bool> found(Tsource.size(), false);

	for (size_t i = 0; i < found.size(); i++)
	{
		if (PLFC[i] == kFloatMissing)
		{
			found[i] = true;
		}
	}

	forecast_time ftime = myTargetInfo->Time();
	forecast_type ftype = myTargetInfo->ForecastType();

	/*
	 * Modus operandi:
	 *
	 * 1. Integrate from source level to LCL dry adiabatically
	 *
	 * This can be done always since LCL is known at all grid points
	 * (that have source data values defined).
	 *
	 * 2. Integrate from LCL to LFC moist adiabatically
	 *
	 * Note! For some points integration will fail (no LFC found)
	 *
	 * We stop integrating at first time CAPE area is found!
	 */

	// Get LCL and LFC heights in meters

	auto ZLCL = h->VerticalValue(param("HL-M"), PLCL);
	auto ZLFC = h->VerticalValue(param("HL-M"), PLFC);

	level curLevel = itsBottomLevel;

	auto prevZenvInfo = Fetch(ftime, curLevel, param("HL-M"), ftype, false);
	auto prevTenvInfo = Fetch(ftime, curLevel, param("T-K"), ftype, false);
	auto prevPenvInfo = Fetch(ftime, curLevel, param("P-HPA"), ftype, false);

	std::vector<double> cinh(PLCL.size(), 0);

	size_t foundCount = count(found.begin(), found.end(), true);

	auto Piter = Psource;
	::MultiplyWith(Piter, 100);

	auto PLCLPa = PLCL;
	::MultiplyWith(PLCLPa, 100);

	auto Titer = Tsource;
	auto prevTparcelVec = Tsource;

	curLevel.Value(curLevel.Value() - 1);

	auto hPa100 = h->LevelForHeight(myTargetInfo->Producer(), 100.);

	while (curLevel.Value() > hPa100.first.Value() && foundCount != found.size())
	{
		auto ZenvInfo = Fetch(ftime, curLevel, param("HL-M"), ftype, false);
		auto TenvInfo = Fetch(ftime, curLevel, param("T-K"), ftype, false);
		auto PenvInfo = Fetch(ftime, curLevel, param("P-HPA"), ftype, false);

		vector<double> TparcelVec(Piter.size(), kFloatMissing);

		// Convert pressure to Pa since metutil-library expects that
		auto PenvVec = PenvInfo->Data().Values();
		::MultiplyWith(PenvVec, 100);

		metutil::LiftLCL(&Piter[0], &Titer[0], &PLCLPa[0], &PenvVec[0], &TparcelVec[0], TparcelVec.size());

		int i = -1;

		auto& cinhref = cinh;

		for (auto&& tup : zip_range(cinhref, VEC(TenvInfo), VEC(prevTenvInfo), VEC(PenvInfo), VEC(prevPenvInfo),
		                            VEC(ZenvInfo), VEC(prevZenvInfo), TparcelVec, prevTparcelVec, Psource))
		{
			i++;

			if (found[i]) continue;

			double& cin = tup.get<0>();

			double Tenv = tup.get<1>();  // K
			assert(Tenv >= 100.);

			double prevTenv = tup.get<2>();

			double Penv = tup.get<3>();  // hPa
			assert(Penv < 1200.);

			double prevPenv = tup.get<4>();

			double Zenv = tup.get<5>();      // m
			double prevZenv = tup.get<6>();  // m

			double Tparcel = tup.get<7>();  // K
			assert(Tparcel >= 100.);

			double prevTparcel = tup.get<8>();  // K

			double Psrc = tup.get<9>();

			if (Penv > Psrc)
			{
				// Have not reached source level yet
				continue;
			}
			else if (Penv <= PLFC[i])
			{
				// reached max height

				found[i] = true;

				if (prevTparcel == kFloatMissing || prevPenv == kFloatMissing || prevTenv == kFloatMissing)
				{
					continue;
				}

				// Integrate the final piece from previous level to LFC level

				// First get LFC height in meters
				Zenv = himan::numerical_functions::interpolation::Linear(PLFC[i], prevPenv, Penv, prevZenv, Zenv);

				// LFC environment temperature value
				Tenv = himan::numerical_functions::interpolation::Linear(PLFC[i], prevPenv, Penv, prevTenv, Tenv);

				// LFC T parcel value
				Tparcel =
				    himan::numerical_functions::interpolation::Linear(PLFC[i], prevPenv, Penv, prevTparcel, Tparcel);

				Penv = PLFC[i];
				assert(Zenv > prevZenv);
			}

			if (Tparcel == kFloatMissing)
			{
				continue;
			}

			if (Penv < PLCL[i])
			{
				// Above LCL, switch to virtual temperature
				Tparcel = metutil::VirtualTemperature_(Tparcel, Penv * 100);
				Tenv = metutil::VirtualTemperature_(Tenv, Penv * 100);
			}

			cin += CAPE::CalcCIN(Tenv, prevTenv, Tparcel, prevTparcel, Penv, prevPenv, Zenv, prevZenv);
			assert(cin <= 0);
		}

		foundCount = count(found.begin(), found.end(), true);

		itsLogger.Trace("CIN read for " + boost::lexical_cast<string>(foundCount) + "/" +
		                boost::lexical_cast<string>(found.size()) + " gridpoints");

		curLevel.Value(curLevel.Value() - 1);

		prevZenvInfo = ZenvInfo;
		prevTenvInfo = TenvInfo;
		prevPenvInfo = PenvInfo;
		prevTparcelVec = TparcelVec;

		for (size_t i = 0; i < Titer.size(); i++)
		{
			if (TparcelVec[i] != kFloatMissing && PenvVec[i] != kFloatMissing)
			{
				Titer[i] = TparcelVec[i];
				Piter[i] = PenvVec[i];
			}

			if (found[i]) Titer[i] = kFloatMissing;  // by setting this we prevent MoistLift to integrate particle
		}
	}

	myTargetInfo->Param(CINParam);
	myTargetInfo->Data().Set(cinh);
}

void cape::GetCAPE(shared_ptr<info> myTargetInfo, const pair<vector<double>, vector<double>>& LFC, param ELTParam,
                   param ELPParam, param ELZParam, param CAPEParam, param CAPE1040Param, param CAPE3kmParam)
{
#ifdef HAVE_CUDA
	if (itsConfiguration->UseCuda())
	{
		cape_cuda::GetCAPEGPU(itsConfiguration, myTargetInfo, LFC.first, LFC.second, ELTParam, ELPParam, CAPEParam,
		                      CAPE1040Param, CAPE3kmParam);
	}
	else
#endif
	{
		GetCAPECPU(myTargetInfo, LFC.first, LFC.second, ELTParam, ELPParam, CAPEParam, CAPE1040Param, CAPE3kmParam);
	}

	auto h = GET_PLUGIN(hitool);

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());
	h->HeightUnit(kHPa);

	myTargetInfo->Param(ELPParam);
	auto height = h->VerticalValue(param("HL-M"), VEC(myTargetInfo));

	myTargetInfo->Param(ELZParam);
	myTargetInfo->Data().Set(height);
}

void cape::GetCAPECPU(shared_ptr<info> myTargetInfo, const vector<double>& T, const vector<double>& P, param ELTParam,
                      param ELPParam, param CAPEParam, param CAPE1040Param, param CAPE3kmParam)
{
	assert(T.size() == P.size());

	auto h = GET_PLUGIN(hitool);

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());
	h->HeightUnit(kHPa);

	// Found count determines if we have calculated all three CAPE variation for a single grid point
	vector<unsigned char> found(T.size(), 0);

	vector<double> CAPE(T.size(), 0);
	vector<double> CAPE1040(T.size(), 0);
	vector<double> CAPE3km(T.size(), 0);
	vector<double> ELT(T.size(), kFloatMissing);
	vector<double> ELP(T.size(), kFloatMissing);

	// Unlike LCL, LFC is *not* found for all grid points

	size_t foundCount = 0;

	for (size_t i = 0; i < P.size(); i++)
	{
		if (P[i] == kFloatMissing)
		{
			found[i] |= FCAPE;
			foundCount++;
		}
	}

	// For each grid point find the hybrid level that's below LFC and then pick the lowest level
	// among all grid points

	auto levels = h->LevelForHeight(myTargetInfo->Producer(), ::Max(P));

	level curLevel = levels.first;

	auto prevZenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("HL-M"), myTargetInfo->ForecastType(), false);
	auto prevTenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("T-K"), myTargetInfo->ForecastType(), false);
	auto prevPenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);

	curLevel.Value(curLevel.Value());

	auto Piter = P, Titer = T;  // integration variables
	auto prevTparcelVec = Titer;

	// Convert pressure to Pa since metutil-library expects that
	::MultiplyWith(Piter, 100);

	info_t TenvInfo, PenvInfo, ZenvInfo;

	auto hPa100 = h->LevelForHeight(myTargetInfo->Producer(), 100.);

	while (curLevel.Value() > hPa100.first.Value() && foundCount != found.size())
	{
		// Get environment temperature, pressure and height values for this level
		PenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);
		TenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("T-K"), myTargetInfo->ForecastType(), false);
		ZenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("HL-M"), myTargetInfo->ForecastType(), false);

		// Convert pressure to Pa since metutil-library expects that
		auto PenvVec = PenvInfo->Data().Values();
		::MultiplyWith(PenvVec, 100);

		vector<double> TparcelVec(P.size(), kFloatMissing);

		::MoistLift(&Piter[0], &Titer[0], &PenvVec[0], &TparcelVec[0], TparcelVec.size());

		int i = -1;
		for (auto&& tup : zip_range(VEC(PenvInfo), VEC(ZenvInfo), VEC(prevZenvInfo), VEC(prevTenvInfo),
		                            VEC(prevPenvInfo), VEC(TenvInfo), TparcelVec, prevTparcelVec))
		{
			i++;

			double Tenv = tup.get<5>();  // K
			assert(Tenv > 100.);

			double prevTenv = tup.get<3>();  // K
			assert(prevTenv > 100.);

			double Penv = tup.get<0>();  // hPa
			assert(Penv < 1200.);

			double prevPenv = tup.get<4>();  // hPa
			assert(prevPenv < 1200.);

			double Zenv = tup.get<1>();      // m
			double prevZenv = tup.get<2>();  // m

			double Tparcel = tup.get<6>();  // K
			assert(Tparcel > 100. || Tparcel == kFloatMissing);

			double prevTparcel = tup.get<7>();  // K
			assert(prevTparcel > 100. || Tparcel == kFloatMissing);

			if (found[i] & FCAPE)
			{
				continue;
			}
			else if (Penv == kFloatMissing || Tenv == kFloatMissing || Zenv == kFloatMissing ||
			         prevZenv == kFloatMissing || Tparcel == kFloatMissing || Penv > P[i])
			{
				// Missing data or current grid point is below LFC
				continue;
			}

			// When rising above LFC, get accurate value of Tenv at that level so that even small amounts of CAPE
			// (and EL!) values can be determined.

			if (prevTparcel == kFloatMissing && Tparcel != kFloatMissing)
			{
				prevTenv = himan::numerical_functions::interpolation::Linear(P[i], prevPenv, Penv, prevTenv, Tenv);
				prevZenv = himan::numerical_functions::interpolation::Linear(P[i], prevPenv, Penv, prevZenv, Zenv);
				prevPenv = P[i];     // LFC pressure
				prevTparcel = T[i];  // LFC temperature

				// If LFC was found close to lower hybrid level, the linear interpolation and moist lift will result
				// to same values. In this case CAPE integration fails as there is no area formed between environment
				// and parcel temperature. The result for this is that LFC is found but EL is not found. To prevent
				// this, warm the parcel value just slightly so that a miniscule CAPE area is formed and EL is found.

				if (fabs(prevTparcel - prevTenv) < 0.0001)
				{
					prevTparcel += 0.0001;
				}
			}

			if (curLevel.Value() < 85 && (Tenv - Tparcel) > 25.)
			{
				// Temperature gap between environment and parcel too large --> abort search.
				// Only for values higher in the atmosphere, to avoid the effects of inversion

				found[i] |= FCAPE;
				continue;
			}

			if (prevZenv >= 3000. && Zenv >= 3000.)
			{
				found[i] |= FCAPE3km;
			}

			if ((found[i] & FCAPE3km) == 0)
			{
				double C = CAPE::CalcCAPE3km(Tenv, prevTenv, Tparcel, prevTparcel, Penv, prevPenv, Zenv, prevZenv);

				CAPE3km[i] += C;

				assert(CAPE3km[i] < 3000.);  // 3000J/kg, not 3000m
				assert(CAPE3km[i] >= 0);
			}

			double C = CAPE::CalcCAPE1040(Tenv, prevTenv, Tparcel, prevTparcel, Penv, prevPenv, Zenv, prevZenv);

			CAPE1040[i] += C;

			assert(CAPE1040[i] < 5000.);
			assert(CAPE1040[i] >= 0);

			double CAPEval, ELTval, ELPval;

			CAPE::CalcCAPE(Tenv, prevTenv, Tparcel, prevTparcel, Penv, prevPenv, Zenv, prevZenv, CAPEval, ELTval,
			               ELPval);

			CAPE[i] += CAPEval;

			assert(CAPEval >= 0.);
			assert(CAPE[i] < 8000);

			if (ELTval != kFloatMissing)
			{
				ELT[i] = ELTval;
				ELP[i] = ELPval;
			}
		}

		curLevel.Value(curLevel.Value() - 1);

		foundCount = 0;
		for (auto& val : found)
		{
			if (val & FCAPE) foundCount++;
		}

		itsLogger.Trace("CAPE read for " + boost::lexical_cast<string>(foundCount) + "/" +
		                boost::lexical_cast<string>(found.size()) + " gridpoints");
		prevZenvInfo = ZenvInfo;
		prevTenvInfo = TenvInfo;
		prevPenvInfo = PenvInfo;
		prevTparcelVec = TparcelVec;
	}

	// If the CAPE area is continued all the way to level 60 and beyond, we don't have an EL for that
	// (since integration is forcefully stopped)
	// In this case level 60 = EL

	for (size_t i = 0; i < CAPE.size(); i++)
	{
		if (CAPE[i] > 0 && ELT[i] == kFloatMissing)
		{
			TenvInfo->LocationIndex(i);
			PenvInfo->LocationIndex(i);

			ELT[i] = TenvInfo->Value();
			ELP[i] = PenvInfo->Value();
		}
	}

	myTargetInfo->Param(ELTParam);
	myTargetInfo->Data().Set(ELT);

	myTargetInfo->Param(ELPParam);
	myTargetInfo->Data().Set(ELP);

	myTargetInfo->Param(CAPEParam);
	myTargetInfo->Data().Set(CAPE);

	myTargetInfo->Param(CAPE1040Param);
	myTargetInfo->Data().Set(CAPE1040);

	myTargetInfo->Param(CAPE3kmParam);
	myTargetInfo->Data().Set(CAPE3km);
}

pair<vector<double>, vector<double>> cape::GetLFC(shared_ptr<info> myTargetInfo, vector<double>& T, vector<double>& P)
{
	auto h = GET_PLUGIN(hitool);

	assert(T.size() == P.size());

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());
	h->HeightUnit(kHPa);

	itsLogger.Trace("Searching environment temperature for starting pressure");

	vector<double> TenvLCL;

	try
	{
		TenvLCL = h->VerticalValue(param("T-K"), P);
	}
	catch (const HPExceptionType& e)
	{
		if (e == kFileDataNotFound)
		{
			return make_pair(vector<double>(), vector<double>());
		}

		throw;
	}

#ifdef HAVE_CUDA
	if (itsConfiguration->UseCuda())
	{
		return cape_cuda::GetLFCGPU(itsConfiguration, myTargetInfo, T, P, TenvLCL);
	}
	else
#endif
	{
		return GetLFCCPU(myTargetInfo, T, P, TenvLCL);
	}
}

pair<vector<double>, vector<double>> cape::GetLFCCPU(shared_ptr<info> myTargetInfo, vector<double>& T,
                                                     vector<double>& P, vector<double>& TenvLCL)
{
	auto h = GET_PLUGIN(hitool);

	assert(T.size() == P.size());

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());
	h->HeightUnit(kHPa);

	auto Piter = P, Titer = T;  // integration variables

	// Convert pressure to Pa since metutil-library expects that
	::MultiplyWith(Piter, 100);

	vector<bool> found(T.size(), false);

	vector<double> LFCT(T.size(), kFloatMissing);
	vector<double> LFCP(T.size(), kFloatMissing);

	for (size_t i = 0; i < TenvLCL.size(); i++)
	{
		// Require dry lifted parcel to be just a fraction higher
		// than environment to be accepted as LFC level.
		// This requirement is important later when CAPE integration
		// starts.

		if ((T[i] - TenvLCL[i]) > 0.001)
		{
			found[i] = true;
			LFCT[i] = T[i];
			LFCP[i] = P[i];
			Piter[i] = kFloatMissing;
		}
	}

	size_t foundCount = count(found.begin(), found.end(), true);

	itsLogger.Debug("Found " + boost::lexical_cast<string>(foundCount) + " gridpoints that have LCL=LFC");

	// For each grid point find the hybrid level that's below LCL and then pick the lowest level
	// among all grid points; most commonly it's the lowest hybrid level

	auto levels = h->LevelForHeight(myTargetInfo->Producer(), ::Max(P));
	level curLevel = levels.first;

	auto prevPenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);
	auto prevTenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("T-K"), myTargetInfo->ForecastType(), false);

	curLevel.Value(curLevel.Value() - 1);

	auto hPa150 = h->LevelForHeight(myTargetInfo->Producer(), 150.);
	auto hPa450 = h->LevelForHeight(myTargetInfo->Producer(), 450.);
	vector<double> prevTparcelVec(P.size(), kFloatMissing);

	while (curLevel.Value() > hPa150.first.Value() && foundCount != found.size())
	{
		// Get environment temperature and pressure values for this level
		auto TenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("T-K"), myTargetInfo->ForecastType(), false);
		auto PenvInfo = Fetch(myTargetInfo->Time(), curLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);

		// Convert pressure to Pa since metutil-library expects that
		auto PenvVec = PenvInfo->Data().Values();
		::MultiplyWith(PenvVec, 100);

		// Lift the particle from previous level to this level. In the first revolution
		// of this loop the starting level is LCL. If target level level is below current level
		// (ie. we would be lowering the particle) missing value is returned.

		vector<double> TparcelVec(P.size(), kFloatMissing);

		::MoistLift(&Piter[0], &Titer[0], &PenvVec[0], &TparcelVec[0], TparcelVec.size());

		double scale = 1;
		if (prevPenvInfo->Param().Name() == "P-PA") scale = 0.01;

		int i = -1;
		for (auto&& tup : zip_range(VEC(TenvInfo), VEC(PenvInfo), VEC(prevPenvInfo), VEC(prevTenvInfo), TparcelVec,
		                            prevTparcelVec, LFCT, LFCP))
		{
			i++;

			if (found[i]) continue;

			double Tenv = tup.get<0>();  // K
			assert(Tenv > 100.);

			double Penv = tup.get<1>();  // hPa
			assert(Penv < 1200.);
			assert(P[i] < 1200.);

			double prevPenv = tup.get<2>() * scale;
			assert(prevPenv < 1200.);

			double prevTenv = tup.get<3>();  // K
			assert(prevTenv > 100.);

			double Tparcel = tup.get<4>();  // K
			assert(Tparcel > 100.);

			double prevTparcel = tup.get<5>();  // K
			assert(Tparcel > 100.);

			double& Tresult = tup.get<6>();
			double& Presult = tup.get<7>();

			if (Tparcel == kFloatMissing || Penv > P[i] + 30)
			{
				continue;
			}

			if (Tparcel > Tenv)
			{
				// Parcel is now warmer than environment, we have found LFC and entering CAPE zone

				found[i] = true;

				if (prevTparcel == kFloatMissing)
				{
					// Previous value is unknown: perhaps LFC is found very close to ground?
					// Use LCL for previous value.
					prevTparcel = T[i];
				}

				if (fabs(prevTparcel - prevTenv) < 0.0001)
				{
					Tresult = Tparcel;
					Presult = Penv;
				}
				else
				{
					auto intersection =
					    CAPE::GetPointOfIntersection(point(Tenv, Penv), point(prevTenv, prevPenv), point(Tparcel, Penv),
					                                 point(prevTparcel, prevPenv));
					Tresult = intersection.X();
					Presult = intersection.Y();

					if (Tresult == kFloatMissing)
					{
						// Intersection not found, use exact level value
						Tresult = Tenv;
						Presult = Penv;
					}
				}

				assert(Tresult != kFloatMissing);
				assert(Presult != kFloatMissing);
			}
			else if (curLevel.Value() < hPa450.first.Value() && (Tenv - Tparcel) > 30.)
			{
				// Temperature gap between environment and parcel too large --> abort search.
				// Only for values higher in the atmosphere, to avoid the effects of inversion

				found[i] = true;
			}
		}

		curLevel.Value(curLevel.Value() - 1);

		foundCount = count(found.begin(), found.end(), true);
		itsLogger.Trace("LFC processed for " + boost::lexical_cast<string>(foundCount) + "/" +
		                boost::lexical_cast<string>(found.size()) + " grid points");

		prevPenvInfo = PenvInfo;
		prevTenvInfo = TenvInfo;

		for (size_t i = 0; i < Titer.size(); i++)
		{
			if (found[i]) Titer[i] = kFloatMissing;  // by setting this we prevent MoistLift to integrate particle
		}

		prevTparcelVec = TparcelVec;
	}

	return make_pair(LFCT, LFCP);
}

pair<vector<double>, vector<double>> cape::GetLCL(shared_ptr<info> myTargetInfo, const cape_source& sourceValues)
{
	vector<double> TLCL(get<0>(sourceValues).size(), kFloatMissing);
	vector<double> PLCL = TLCL;

	// Need surface pressure

	double Pscale = 100.;  // P should be Pa

	for (auto&& tup : zip_range(get<0>(sourceValues), get<1>(sourceValues), get<2>(sourceValues), TLCL, PLCL))
	{
		double T = tup.get<0>();
		double TD = tup.get<1>();
		double P = tup.get<2>() * Pscale;  // Pa
		double& Tresult = tup.get<3>();
		double& Presult = tup.get<4>();

		auto lcl = metutil::LCLA_(P, T, TD);

		Tresult = lcl.T;  // K

		if (lcl.P != kFloatMissing)
		{
			Presult = 0.01 * ((lcl.P > P) ? P : lcl.P);  // hPa
		}
	}

	for (auto& val : PLCL)
	{
		val = fmax(val, 250.);
	}

	return make_pair(TLCL, PLCL);
}

cape_source cape::GetSurfaceValues(shared_ptr<info> myTargetInfo)
{
	/*
	 * 1. Get temperature and relative humidity from lowest hybrid level.
	 * 2. Calculate dewpoint
	 * 3. Return temperature and dewpoint
	 */

	auto TInfo = Fetch(myTargetInfo->Time(), itsBottomLevel, param("T-K"), myTargetInfo->ForecastType(), false);
	auto RHInfo = Fetch(myTargetInfo->Time(), itsBottomLevel, param("RH-PRCNT"), myTargetInfo->ForecastType(), false);
	auto PInfo = Fetch(myTargetInfo->Time(), itsBottomLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);

	if (!TInfo || !RHInfo || !PInfo)
	{
		return make_tuple(vector<double>(), vector<double>(), vector<double>());
	}

	auto T = VEC(TInfo);
	auto RH = VEC(RHInfo);

	vector<double> TD(T.size(), kFloatMissing);

	for (size_t i = 0; i < TD.size(); i++)
	{
		if (T[i] != kFloatMissing && RH[i] != kFloatMissing)
		{
			TD[i] = metutil::DewPointFromRH_(T[i], RH[i]);
		}
	}

	return make_tuple(T, TD, VEC(PInfo));
}

cape_source cape::Get500mMixingRatioValues(shared_ptr<info> myTargetInfo)
{
/*
 * 1. Calculate potential temperature and mixing ratio for vertical profile
 *    0...500m for every 2 hPa
 * 2. Take an average from all values
 * 3. Calculate temperature from potential temperature, and dewpoint temperature
 *    from temperature and mixing ratio
 * 4. Return the two calculated values
 */

#ifdef HAVE_CUDA
	if (itsConfiguration->UseCuda())
	{
		return cape_cuda::Get500mMixingRatioValuesGPU(itsConfiguration, myTargetInfo);
	}
	else
#endif
	{
		return Get500mMixingRatioValuesCPU(myTargetInfo);
	}
}

cape_source cape::Get500mMixingRatioValuesCPU(shared_ptr<info> myTargetInfo)
{
	modifier_mean tp, mr;
	level curLevel = itsBottomLevel;

	auto h = GET_PLUGIN(hitool);

	h->Configuration(itsConfiguration);
	h->Time(myTargetInfo->Time());
	h->ForecastType(myTargetInfo->ForecastType());

	tp.HeightInMeters(false);
	mr.HeightInMeters(false);

	auto PInfo = Fetch(myTargetInfo->Time(), curLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);

	if (!PInfo)
	{
		return make_tuple(vector<double>(), vector<double>(), vector<double>());
	}
	else
	{
		// Himan specialty: empty data grid

		size_t miss = 0;
		for (auto& val : VEC(PInfo))
		{
			if (val == kFloatMissing) miss++;
		}

		if (PInfo->Data().MissingCount() == PInfo->Data().Size())
		{
			return make_tuple(vector<double>(), vector<double>(), vector<double>());
		}
	}

	auto P = PInfo->Data().Values();

	auto P500m = h->VerticalValue(param("P-HPA"), 500.);

	h->HeightUnit(kHPa);

	tp.LowerHeight(P);
	mr.LowerHeight(P);

	tp.UpperHeight(P500m);
	mr.UpperHeight(P500m);

	vector<bool> found(myTargetInfo->Data().Size(), false);
	size_t foundCount = 0;

	while (foundCount != found.size())
	{
		auto T = h->VerticalValue(param("T-K"), P);
		auto RH = h->VerticalValue(param("RH-PRCNT"), P);

		vector<double> Tpot(T.size(), kFloatMissing);
		vector<double> MR(T.size(), kFloatMissing);

		for (size_t i = 0; i < T.size(); i++)
		{
			if (found[i]) continue;
			if (T[i] == kFloatMissing || P[i] == kFloatMissing || RH[i] == kFloatMissing) continue;

			assert(T[i] > 150 && T[i] < 350);
			assert(P[i] > 100 && P[i] < 1500);
			assert(RH[i] > 0 && RH[i] < 102);

			Tpot[i] = metutil::Theta_(T[i], 100 * P[i]);
			MR[i] = metutil::smarttool::MixingRatio_(T[i], RH[i], 100 * P[i]);
		}

		tp.Process(Tpot, P);
		mr.Process(MR, P);

		foundCount = tp.HeightsCrossed();

		assert(tp.HeightsCrossed() == mr.HeightsCrossed());

		itsLogger.Debug("Data read " + boost::lexical_cast<string>(foundCount) + "/" +
		                boost::lexical_cast<string>(found.size()) + " gridpoints");

		for (size_t i = 0; i < found.size(); i++)
		{
			assert((P[i] > 100 && P[i] < 1500) || P[i] == kFloatMissing);

			if (found[i])
			{
				P[i] = kFloatMissing;  // disable processing of this
			}
			else if (P[i] != kFloatMissing)
			{
				P[i] -= 2.0;
			}
		}
	}

	auto Tpot = tp.Result();
	auto MR = mr.Result();

	auto Psurf = Fetch(myTargetInfo->Time(), itsBottomLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);
	P = Psurf->Data().Values();

	vector<double> T(Tpot.size(), kFloatMissing);

	for (size_t i = 0; i < Tpot.size(); i++)
	{
		assert((P[i] > 100 && P[i] < 1500) || P[i] == kFloatMissing);
		if (Tpot[i] != kFloatMissing && P[i] != kFloatMissing)
		{
			T[i] = Tpot[i] * pow((P[i] / 1000.), 0.2854);
		}
	}

	vector<double> TD(T.size(), kFloatMissing);

	for (size_t i = 0; i < MR.size(); i++)
	{
		if (T[i] != kFloatMissing && MR[i] != kFloatMissing && P[i] != kFloatMissing)
		{
			double Es = metutil::Es_(T[i]);  // Saturated water vapor pressure
			double E = metutil::E_(MR[i], 100 * P[i]);

			double RH = E / Es * 100;
			TD[i] = metutil::DewPointFromRH_(T[i], RH);
		}
	}

	return make_tuple(T, TD, P);
}

cape_source cape::GetHighestThetaEValues(shared_ptr<info> myTargetInfo)
{
/*
 * 1. Calculate equivalent potential temperature for all hybrid levels
 *    below 600hPa
 * 2. Take temperature and relative humidity from the level that had
 *    highest theta e
 * 3. Calculate dewpoint temperature from temperature and relative humidity.
 * 4. Return temperature and dewpoint
 */

#ifdef HAVE_CUDA
	if (itsConfiguration->UseCuda())
	{
		return cape_cuda::GetHighestThetaEValuesGPU(itsConfiguration, myTargetInfo);
	}
	else
#endif
	{
		return GetHighestThetaEValuesCPU(myTargetInfo);
	}
}

cape_source cape::GetHighestThetaEValuesCPU(shared_ptr<info> myTargetInfo)
{
	vector<bool> found(myTargetInfo->Data().Size(), false);

	vector<double> maxThetaE(myTargetInfo->Data().Size(), -1);
	vector<double> Ttheta(myTargetInfo->Data().Size(), kFloatMissing);
	auto TDtheta = Ttheta;
	auto Ptheta = Ttheta;

	level curLevel = itsBottomLevel;

	info_t prevTInfo, prevRHInfo, prevPInfo;

	while (true)
	{
		auto TInfo = Fetch(myTargetInfo->Time(), curLevel, param("T-K"), myTargetInfo->ForecastType(), false);
		auto RHInfo = Fetch(myTargetInfo->Time(), curLevel, param("RH-PRCNT"), myTargetInfo->ForecastType(), false);
		auto PInfo = Fetch(myTargetInfo->Time(), curLevel, param("P-HPA"), myTargetInfo->ForecastType(), false);

		if (!TInfo || !RHInfo || !PInfo)
		{
			return make_tuple(vector<double>(), vector<double>(), vector<double>());
		}

		int i = -1;

		for (auto&& tup : zip_range(VEC(TInfo), VEC(RHInfo), VEC(PInfo), maxThetaE, Ttheta, TDtheta, Ptheta))
		{
			i++;

			if (found[i]) continue;

			double T = tup.get<0>();
			double RH = tup.get<1>();
			double P = tup.get<2>();
			double& refThetaE = tup.get<3>();
			double& Tresult = tup.get<4>();
			double& TDresult = tup.get<5>();
			double& Presult = tup.get<6>();

			if (P == kFloatMissing)
			{
				found[i] = true;
				continue;
			}

			if (P < 600.)
			{
				found[i] = true;  // Make sure this is the last time we access this grid point

				if (!prevPInfo || !prevTInfo || !prevRHInfo)
				{
					// Lowest grid point located above 600hPa, hmm...
					continue;
				}

				// Cut search if reach level 600hPa
				prevPInfo->LocationIndex(i);
				prevTInfo->LocationIndex(i);
				prevRHInfo->LocationIndex(i);

				// Linearly interpolate temperature and humidity values to 600hPa, to check
				// if highest theta e is found there

				T = himan::numerical_functions::interpolation::Linear(600., P, prevPInfo->Value(), T,
				                                                      prevTInfo->Value());
				RH = himan::numerical_functions::interpolation::Linear(600., P, prevPInfo->Value(), RH,
				                                                       prevRHInfo->Value());

				P = 600.;
			}

			double TD = metutil::DewPointFromRH_(T, RH);
			double ThetaE = metutil::smarttool::ThetaE_(T, RH, P * 100);
			assert(ThetaE >= 0);

			if (ThetaE >= refThetaE)
			{
				refThetaE = ThetaE;
				Tresult = T;
				TDresult = TD;
				Presult = P;

				assert(TDresult > 100);
			}
		}

		size_t foundCount = count(found.begin(), found.end(), true);

		if (foundCount == found.size())
		{
			break;
		}

		itsLogger.Trace("Max ThetaE processed for " + boost::lexical_cast<string>(foundCount) + "/" +
		                boost::lexical_cast<string>(found.size()) + " grid points");

		curLevel.Value(curLevel.Value() - 1);

		prevPInfo = PInfo;
		prevTInfo = TInfo;
		prevRHInfo = RHInfo;
	}

	return make_tuple(Ttheta, TDtheta, Ptheta);
}