/*! @file stats.cc
 *  @brief Statistical measures calculation.
 *  @author Markovtsev Vadim <v.markovtsev@samsung.com>
 *  @version 1.0
 *
 *  @section Notes
 *  This code partially conforms to Google C++ Style Guide.
 *
 *  @section Copyright
 *  Copyright © 2013 Samsung R&D Institute Russia
 *
 *  @section License
 *  Licensed to the Apache Software Foundation (ASF) under one
 *  or more contributor license agreements.  See the NOTICE file
 *  distributed with this work for additional information
 *  regarding copyright ownership.  The ASF licenses this file
 *  to you 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.
 */

#include "src/transforms/stats.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#include <boost/regex.hpp>
#pragma GCC diagnostic pop
#include <cmath>
#include <simd/instruction_set.h>

namespace sound_feature_extraction {
namespace transforms {

std::set<StatsType> Parse(const std::string& value,
                          identity<std::set<StatsType>>) {
  static const std::unordered_map<std::string, StatsType> map {
    { internal::kStatsTypeAverageStr, kStatsTypeAverage },
    { internal::kStatsTypeStdDeviationStr, kStatsTypeStdDeviation },
    { internal::kStatsTypeSkewnessStr, kStatsTypeSkewness },
    { internal::kStatsTypeKurtosisStr, kStatsTypeKurtosis }
  };

  static const boost::regex all_regex("^\\s*(\\w+\\s*(\\s|$))+");
  boost::smatch match;
  if (!boost::regex_match(value, match, all_regex)) {
    throw InvalidParameterValueException();
  }

  if (value == internal::kStatsTypeAllStr) {
    return { kStatsTypeAverage, kStatsTypeStdDeviation, kStatsTypeSkewness,
             kStatsTypeKurtosis };
  }
  std::set<StatsType> ret;
  std::transform(boost::sregex_token_iterator(value.begin(), value.end(),
                                            boost::regex("\\s*(\\w+)\\s*"),
                                            1),
                 boost::sregex_token_iterator(),
                 std::inserter(ret, ret.begin()),
                 [](const std::string& subval) {
    auto stit = map.find(subval);
    if (stit == map.end()) {
      throw InvalidParameterValueException();
    }
    return stit->second;
  });

  return ret;
}

const std::unordered_map<int, Stats::CalculateFunc> Stats::kStatsFuncs {
  { kStatsTypeAverage, Stats::CalculateAverage },
  { kStatsTypeStdDeviation, Stats::CalculateStdDeviation },
  { kStatsTypeSkewness, Stats::CalculateSkewness },
  { kStatsTypeKurtosis, Stats::CalculateKurtosis }
};

Stats::Stats()
    : types_(kDefaultStatsTypes()),
      interval_(kDefaultInterval),
      overlap_(kDefaultOverlap) {
}

ALWAYS_VALID_TP(Stats, types)

bool Stats::validate_interval(const int& value) noexcept {
  return value >= 2;
}

bool Stats::validate_overlap(const int& value) noexcept {
  return value >= 0;
}

void Stats::Initialize() const {
  if (interval_ > 0 && overlap_ >= interval_) {
    throw InvalidParameterValueException("overlap", std::to_string(overlap_),
                                         HostName());
  }
  if (input_format_->Size() < static_cast<size_t>(interval_)) {
    throw UnableToCalculateStatsException(input_format_->Size(), interval_);
  }
}

size_t Stats::OnInputFormatChanged(size_t buffersCount) {
  if (interval_ != 0) {
    int step = interval_ - overlap_;
    auto ratio = (input_format_->Size() - interval_) / step + 1;
    if (((input_format_->Size() - interval_) % step) == 0) {
      output_format_->SetSize(ratio * types_.size());
    } else {
      output_format_->SetSize((ratio + 1) * types_.size());
    }
  } else {
    output_format_->SetSize(types_.size());
  }
  return buffersCount;
}

void Stats::Do(const float* in, float* out) const noexcept {
  float rawMoments[kStatsTypeCount];
  if (interval_ == 0) {
    CalculateRawMoments(true, in, 0, input_format_->Size(), rawMoments);
    Calculate(rawMoments, out);
  } else {
    size_t i;
    size_t step = interval_ - overlap_;
    for (i = 0; i < input_format_->Size() - interval_ + 1; i += step) {
      CalculateRawMoments(true, in, i, interval_, rawMoments);
      Calculate(rawMoments, out + i / step * types_.size());
    }
    if ((input_format_->Size() - interval_) % step != 0) {
      int index = input_format_->Size() - interval_;
      CalculateRawMoments(true, in, index, interval_, rawMoments);
      Calculate(rawMoments, out + i / step * types_.size());
    }
  }
}

void Stats::Calculate(const float* rawMoments, float* out) const noexcept {
  for (auto stat : types_) {
    int sind = 0;
    int istat = stat;
    while (istat >>= 1) {
      sind++;
    }
    out[sind] = kStatsFuncs.find(stat)->second(rawMoments);
  }
}

void Stats::CalculateRawMoments(bool simd, const float* in, int startIndex,
                                int length, float* rawMoments) noexcept {
  float avg1 = 0, avg2 = 0, avg3 = 0, avg4 = 0;
  auto end_index = startIndex + length;
  if (simd) {
#ifdef __AVX__
    __m256 avg1vec = _mm256_setzero_ps();
    __m256 avg2vec = _mm256_setzero_ps();
    __m256 avg3vec = _mm256_setzero_ps();
    __m256 avg4vec = _mm256_setzero_ps();
    for (int i = startIndex; i < end_index - 7; i+=8) {
      __m256 val = _mm256_loadu_ps(in + i);
      avg1vec = _mm256_add_ps(avg1vec, val);
      __m256 val2 = _mm256_mul_ps(val, val);
      avg2vec = _mm256_add_ps(avg2vec, val2);
      val = _mm256_mul_ps(val2, val);
      avg3vec = _mm256_add_ps(avg3vec, val);
      val2 = _mm256_mul_ps(val2, val2);
      avg4vec = _mm256_add_ps(avg4vec, val2);
    }
    avg1vec = _mm256_hadd_ps(avg1vec, avg1vec);
    avg1vec = _mm256_hadd_ps(avg1vec, avg1vec);
    avg1 += _mm256_get_ps(avg1vec, 0);
    avg1 += _mm256_get_ps(avg1vec, 4);
    avg2vec = _mm256_hadd_ps(avg2vec, avg2vec);
    avg2vec = _mm256_hadd_ps(avg2vec, avg2vec);
    avg2 += _mm256_get_ps(avg2vec, 0);
    avg2 += _mm256_get_ps(avg2vec, 4);
    avg3vec = _mm256_hadd_ps(avg3vec, avg3vec);
    avg3vec = _mm256_hadd_ps(avg3vec, avg3vec);
    avg3 += _mm256_get_ps(avg3vec, 0);
    avg3 += _mm256_get_ps(avg3vec, 4);
    avg4vec = _mm256_hadd_ps(avg4vec, avg4vec);
    avg4vec = _mm256_hadd_ps(avg4vec, avg4vec);
    avg4 += _mm256_get_ps(avg4vec, 0);
    avg4 += _mm256_get_ps(avg4vec, 4);
    for (int i = ((end_index) & ~0x7); i < end_index; i++) {
      float v = in[i];
      avg1 += v;
      float v2 = v * v;
      avg2 += v2;
      v *= v2;
      avg3 += v;
      v2 *= v2;
      avg4 += v2;
    }
  } else {
#elif defined(__ARM_NEON__)
    float32x4_t avg1vec = vdupq_n_f32(0);
    float32x4_t avg2vec = vdupq_n_f32(0);
    float32x4_t avg3vec = vdupq_n_f32(0);
    float32x4_t avg4vec = vdupq_n_f32(0);
    for (int i = startIndex; i < end_index - 3; i+=4) {
      float32x4_t val = vld1q_f32(in + i);
      avg1vec = vaddq_f32(avg1vec, val);
      float32x4_t val2 = vmulq_f32(val, val);
      avg2vec = vaddq_f32(avg2vec, val2);
      val = vmulq_f32(val2, val);
      avg3vec = vaddq_f32(avg3vec, val);
      val2 = vmulq_f32(val2, val2);
      avg4vec = vaddq_f32(avg4vec, val2);
    }
    avg1 += vgetq_lane_f32(avg1vec, 0);
    avg1 += vgetq_lane_f32(avg1vec, 1);
    avg1 += vgetq_lane_f32(avg1vec, 2);
    avg1 += vgetq_lane_f32(avg1vec, 3);
    avg2 += vgetq_lane_f32(avg2vec, 0);
    avg2 += vgetq_lane_f32(avg2vec, 1);
    avg2 += vgetq_lane_f32(avg2vec, 2);
    avg2 += vgetq_lane_f32(avg2vec, 3);
    avg3 += vgetq_lane_f32(avg3vec, 0);
    avg3 += vgetq_lane_f32(avg3vec, 1);
    avg3 += vgetq_lane_f32(avg3vec, 2);
    avg3 += vgetq_lane_f32(avg3vec, 3);
    avg4 += vgetq_lane_f32(avg4vec, 0);
    avg4 += vgetq_lane_f32(avg4vec, 1);
    avg4 += vgetq_lane_f32(avg4vec, 2);
    avg4 += vgetq_lane_f32(avg4vec, 3);
    for (int i = ((end_index) & ~0x3); i < end_index; i++) {
      float v = in[i];
      avg1 += v;
      float v2 = v * v;
      avg2 += v2;
      v *= v2;
      avg3 += v;
      v2 *= v2;
      avg4 += v2;
    }
  } else {
#else
  } {
#endif
    for (int i = startIndex; i < end_index; i++) {
      float v = in[i];
      avg1 += v;
      float v2 = v * v;
      avg2 += v2;
      v *= v2;
      avg3 += v;
      v2 *= v2;
      avg4 += v2;
    }
  }
  avg1 /= length;
  avg2 /= length;
  avg3 /= length;
  avg4 /= length;
  rawMoments[0] = avg1;
  rawMoments[1] = avg2;
  rawMoments[2] = avg3;
  rawMoments[3] = avg4;
}

float Stats::CalculateAverage(const float* rawMoments) noexcept {
  return rawMoments[0];
}

float Stats::CalculateStdDeviation(const float* rawMoments) noexcept {
  auto value = rawMoments[1] - rawMoments[0] * rawMoments[0];
  if (value < 0) {
    return 0;
  }
  value = sqrtf(value);
  return value;
}

float Stats::CalculateSkewness(const float* rawMoments) noexcept {
  float avg1 = rawMoments[0];
  float avg2 = rawMoments[1];
  float avg3 = rawMoments[2];
  double u2 = avg2 - avg1 * avg1;
  if (u2 <= 0) {
    return 0;
  }
  double u3 = avg3 - 3 * avg2 * avg1 + 2 * avg1 * avg1 * avg1;
  auto value = u3 / (sqrt(u2) * u2);
  return value;
}

float Stats::CalculateKurtosis(const float* rawMoments) noexcept {
  float avg1 = rawMoments[0];
  float avg2 = rawMoments[1];
  float avg3 = rawMoments[2];
  float avg4 = rawMoments[3];
  double u2 = avg2 - avg1 * avg1;
  if (u2 == 0) {
    return -2.f;
  }
  double u4 = avg4 - 4 * avg3 * avg1 + 6 * avg2 * avg1 * avg1
      - 3 * avg1 * avg1 * avg1 * avg1;
  auto value = u4 / (u2 * u2) - 3;
  return value;
}

RTP(Stats, types)
RTP(Stats, interval)
RTP(Stats, overlap)
REGISTER_TRANSFORM(Stats);

}  // namespace transforms
}  // namespace sound_feature_extraction


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 14:37:45

#include "MSSMatMSUSYEFTHiggs_mAmu_two_scale_initial_guesser.hpp"
#include "MSSMatMSUSYEFTHiggs_mAmu_two_scale_model.hpp"
#include "MSSMatMSUSYEFTHiggs_mAmu_standard_model_two_scale_matching.hpp"
#include "standard_model_two_scale_model.hpp"
#include "lowe.h"
#include "error.hpp"
#include "ew_input.hpp"
#include "wrappers.hpp"

#include <Eigen/Core>

namespace flexiblesusy {

#define INPUTPARAMETER(p) model->get_input().p
#define MODELPARAMETER(p) model->get_##p()
#define SMPARAMETER(p) eft->get_##p()
#define PHASE(p) model->get_##p()
#define LowEnergyConstant(p) Electroweak_constants::p
#define MODEL model

MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser(
   MSSMatMSUSYEFTHiggs_mAmu<Two_scale>* model_,
   standard_model::StandardModel<Two_scale>* eft_,
   const softsusy::QedQcd& qedqcd_,
   const standard_model::Standard_model_low_scale_constraint<Two_scale>& low_constraint_,
   const MSSMatMSUSYEFTHiggs_mAmu_susy_scale_constraint<Two_scale>& susy_constraint_
)
   : Initial_guesser()
   , model(model_)
   , eft(eft_)
   , qedqcd(qedqcd_)
   , low_constraint(low_constraint_)
   , susy_constraint(susy_constraint_)
{
   if (!model)
      throw SetupError("MSSMatMSUSYEFTHiggs_mAmu_initial_guesser: Error: pointer to model"
                       " MSSMatMSUSYEFTHiggs_mAmu<Two_scale> must not be zero");
}

MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::~MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser()
{
}

/**
 * Guesses the DR-bar model parameters by calling
 * guess_susy_parameters() and guess_soft_parameters() .
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::guess()
{
   guess_eft_parameters();
   guess_model_parameters();
}

/**
 * Guesses the effective parameters (gauge, Yukawa couplings) at
 * \f$m_\text{top}^\text{pole}\f$ from the Standard Model gauge
 * couplings and fermion masses.  Threshold corrections are ignored.
 * The user-defined initial guess at the low-scale
 * (InitialGuessAtLowScale) are ignored
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::guess_eft_parameters()
{
   softsusy::QedQcd leAtMt(qedqcd);
   const double mtpole = leAtMt.displayPoleMt();

   mu_guess = leAtMt.displayMass(softsusy::mUp);
   mc_guess = leAtMt.displayMass(softsusy::mCharm);
   mt_guess = model->get_thresholds() > 0 && model->get_threshold_corrections().mt > 0 ?
      leAtMt.displayMass(softsusy::mTop) - 30.0 :
      leAtMt.displayPoleMt();
   md_guess = leAtMt.displayMass(softsusy::mDown);
   ms_guess = leAtMt.displayMass(softsusy::mStrange);
   mb_guess = leAtMt.displayMass(softsusy::mBottom);
   me_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(softsusy::mElectron) :
      leAtMt.displayPoleMel();
   mm_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(softsusy::mMuon) :
      leAtMt.displayPoleMmuon();
   mtau_guess = leAtMt.displayMass(softsusy::mTau);

   // guess gauge couplings at mt
   const auto alpha_sm(leAtMt.guess_alpha_SM5(mtpole));

   eft->set_g1(sqrt(4.0 * Pi * alpha_sm(0)));
   eft->set_g2(sqrt(4.0 * Pi * alpha_sm(1)));
   eft->set_g3(sqrt(4.0 * Pi * alpha_sm(2)));
   eft->set_scale(mtpole);

   eft->set_v(Electroweak_constants::vev);
   eft->set_Yu(ZEROMATRIX(3,3));
   eft->set_Yd(ZEROMATRIX(3,3));
   eft->set_Ye(ZEROMATRIX(3,3));

   eft->set_Yu(0, 0, -Sqrt(2.)* mu_guess/ eft->get_v());
   eft->set_Yu(1, 1, -Sqrt(2.)* mc_guess/ eft->get_v());
   eft->set_Yu(2, 2, -Sqrt(2.)* mt_guess/ eft->get_v());

   eft->set_Yd(0, 0, Sqrt(2.)* md_guess/ eft->get_v());
   eft->set_Yd(1, 1, Sqrt(2.)* ms_guess/ eft->get_v());
   eft->set_Yd(2, 2, Sqrt(2.)* mb_guess/ eft->get_v());

   eft->set_Ye(0, 0, Sqrt(2.)* me_guess/ eft->get_v());
   eft->set_Ye(1, 1, Sqrt(2.)* mm_guess/ eft->get_v());
   eft->set_Ye(2, 2, Sqrt(2.)* mtau_guess/ eft->get_v());

   eft->set_Lambdax(0.12604);
   eft->solve_ewsb_tree_level();

}

void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_DRbar_yukawa_couplings()
{
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
}

/**
 * Calculates the Yukawa couplings Yu of the up-type quarks
 * from the Standard Model up-type quark masses (ignoring threshold
 * corrections).
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_Yu_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> upQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   upQuarksDRbar(0,0) = mu_guess;
   upQuarksDRbar(1,1) = mc_guess;
   upQuarksDRbar(2,2) = mt_guess;

}

/**
 * Calculates the Yukawa couplings Yd of the down-type
 * quarks from the Standard Model down-type quark masses (ignoring
 * threshold corrections).
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_Yd_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   downQuarksDRbar(0,0) = md_guess;
   downQuarksDRbar(1,1) = ms_guess;
   downQuarksDRbar(2,2) = mb_guess;

}

/**
 * Calculates the Yukawa couplings Ye of the leptons
 * from the Standard Model down-type lepton masses (ignoring threshold
 * corrections).
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::calculate_Ye_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downLeptonsDRbar(ZEROMATRIXCOMPLEX(3,3));
   downLeptonsDRbar(0,0) = me_guess;
   downLeptonsDRbar(1,1) = mm_guess;
   downLeptonsDRbar(2,2) = mtau_guess;

}

/**
 * Guesses the full model parameters.  At first it runs to the
 * guess of the SUSY-scale (SUSYScaleFirstGuess) and imposes the
 * SUSY scale initial guess and the
 * SUSY-scale constraint (SUSYScaleInput).  Afterwards, it solves the
 * EWSB conditions at the tree-level.
 * Finally the DR-bar mass spectrum is calculated.
 */
void MSSMatMSUSYEFTHiggs_mAmu_standard_model_initial_guesser<Two_scale>::guess_model_parameters()
{
   const double susy_scale_guess = susy_constraint.get_initial_scale_guess();
   const auto scale_getter = [this] () { return susy_constraint.get_scale(); };

   model->set_scale(susy_scale_guess);

   // apply susy-scale first guess
   const auto TanBeta = INPUTPARAMETER(TanBeta);
   const auto M1Input = INPUTPARAMETER(M1Input);
   const auto M2Input = INPUTPARAMETER(M2Input);
   const auto M3Input = INPUTPARAMETER(M3Input);
   const auto mq2Input = INPUTPARAMETER(mq2Input);
   const auto mu2Input = INPUTPARAMETER(mu2Input);
   const auto md2Input = INPUTPARAMETER(md2Input);
   const auto ml2Input = INPUTPARAMETER(ml2Input);
   const auto me2Input = INPUTPARAMETER(me2Input);
   const auto MuInput = INPUTPARAMETER(MuInput);
   const auto mAInput = INPUTPARAMETER(mAInput);
   const auto AuInput = INPUTPARAMETER(AuInput);
   const auto AdInput = INPUTPARAMETER(AdInput);
   const auto AeInput = INPUTPARAMETER(AeInput);
   const auto Yu = MODELPARAMETER(Yu);
   const auto Yd = MODELPARAMETER(Yd);
   const auto Ye = MODELPARAMETER(Ye);

   MODEL->set_vu(Re((TanBeta*LowEnergyConstant(vev))/Sqrt(1 + Sqr(TanBeta))));
   MODEL->set_vd(Re(LowEnergyConstant(vev)/Sqrt(1 + Sqr(TanBeta))));
   MODEL->set_MassB(Re(M1Input));
   MODEL->set_MassWB(Re(M2Input));
   MODEL->set_MassG(Re(M3Input));
   MODEL->set_mq2((mq2Input).real());
   MODEL->set_mu2((mu2Input).real());
   MODEL->set_md2((md2Input).real());
   MODEL->set_ml2((ml2Input).real());
   MODEL->set_me2((me2Input).real());
   MODEL->set_Mu(Re(MuInput));
   MODEL->set_BMu(Re(Sqr(mAInput)/(1/TanBeta + TanBeta)));
   MODEL->set_TYu(((AuInput).cwiseProduct(Yu)).real());
   MODEL->set_TYd(((AdInput).cwiseProduct(Yd)).real());
   MODEL->set_TYe(((AeInput).cwiseProduct(Ye)).real());

   eft->run_to(susy_scale_guess, running_precision);
   eft->calculate_DRbar_masses();

   //get gauge and Yukawa couplings from effective theory
   MSSMatMSUSYEFTHiggs_mAmu_standard_model_matching_up<Two_scale> matching_up;
   matching_up.set_models(eft, model);
   matching_up.set_scale(scale_getter);
   matching_up.match_tree_level();

   model->run_to(susy_scale_guess, running_precision);

   // apply susy-scale constraint
   susy_constraint.set_model(model);
   susy_constraint.apply();

   // apply EWSB constraint
   model->solve_ewsb_tree_level();

   // calculate tree-level spectrum
   model->calculate_DRbar_masses();
}

} // namespace flexiblesusy


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "PointwiseFunctions/AnalyticSolutions/GeneralRelativity/Tov.hpp"

#include <algorithm>
#include <array>
#include <boost/numeric/odeint.hpp>  // IWYU pragma: keep
#include <cmath>
#include <cstddef>
#include <functional>
#include <pup.h>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "Utilities/Gsl.hpp"

// IWYU pragma: no_forward_declare boost::numeric::odeint::controlled_runge_kutta
// IWYU pragma: no_forward_declare EquationsOfState::EquationOfState
// IWYU pragma: no_forward_declare Tensor
// IWYU pragma: no_include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/generation/make_dense_output.hpp>
// IWYU pragma: no_include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>

/// \cond
namespace {

void lindblom_rhs(
    gsl::not_null<std::array<double, 2>*> dvars,
    const std::array<double, 2>& vars, const double log_enthalpy,
    const std::unique_ptr<EquationsOfState::EquationOfState<true, 1>>&
        equation_of_state) noexcept {
  const double& radius_squared = vars[0];
  const double& mass_over_radius = vars[1];
  double& d_radius_squared = (*dvars)[0];
  double& d_mass_over_radius = (*dvars)[1];
  const double specific_enthalpy = std::exp(log_enthalpy);
  const double rest_mass_density =
      get(equation_of_state->rest_mass_density_from_enthalpy(
          Scalar<double>{specific_enthalpy}));
  const double pressure = get(equation_of_state->pressure_from_density(
      Scalar<double>{rest_mass_density}));
  const double energy_density =
      specific_enthalpy * rest_mass_density - pressure;

  // At the center of the star: (u,v) = (0,0)
  if (UNLIKELY((radius_squared == 0.0) and (mass_over_radius == 0.0))) {
    d_radius_squared = -3.0 / (2.0 * M_PI * (energy_density + 3.0 * pressure));
    d_mass_over_radius =
        -2.0 * energy_density / (energy_density + 3.0 * pressure);
  } else {
    const double common_factor =
        (1.0 - 2.0 * mass_over_radius) /
        (4.0 * M_PI * radius_squared * pressure + mass_over_radius);
    d_radius_squared = -2.0 * radius_squared * common_factor;
    d_mass_over_radius =
        -(4.0 * M_PI * radius_squared * energy_density - mass_over_radius) *
        common_factor;
  }
}

class Observer {
 public:
  void operator()(const std::array<double, 2>& vars,
                  const double current_log_enthalpy) noexcept {
    radius.push_back(std::sqrt(vars[0]));
    mass.push_back(std::sqrt(vars[0]) * vars[1]);
    log_enthalpy.push_back(current_log_enthalpy);
  }
  std::vector<double> radius;
  std::vector<double> mass;
  std::vector<double> log_enthalpy;
};

}  // namespace

namespace gr {
namespace Solutions {

TovSolution::TovSolution(
    const std::unique_ptr<EquationsOfState::EquationOfState<true, 1>>&
        equation_of_state,
    const double central_mass_density, const double final_log_enthalpy,
    const double absolute_tolerance, const double relative_tolerance) {
  std::array<double, 2> u_and_v = {{0.0, 0.0}};
  std::array<double, 2> dudh_and_dvdh{};
  const double central_log_enthalpy =
      std::log(get(equation_of_state->specific_enthalpy_from_density(
          Scalar<double>{central_mass_density})));
  lindblom_rhs(&dudh_and_dvdh, u_and_v, central_log_enthalpy,
               equation_of_state);
  const double initial_step = -std::min(std::abs(1.0 / dudh_and_dvdh[0]),
                                        std::abs(1.0 / dudh_and_dvdh[1]));
  using StateDopri5 =
      boost::numeric::odeint::runge_kutta_dopri5<std::array<double, 2>>;
  boost::numeric::odeint::dense_output_runge_kutta<
      boost::numeric::odeint::controlled_runge_kutta<StateDopri5>>
      dopri5 = make_dense_output(absolute_tolerance, relative_tolerance,
                                 StateDopri5{});
  Observer observer{};
  boost::numeric::odeint::integrate_adaptive(
      dopri5,
      [&equation_of_state](const std::array<double, 2>& lindblom_u_and_v,
                           std::array<double, 2>& lindblom_dudh_and_dvdh,
                           const double lindblom_enthalpy) noexcept {
        return lindblom_rhs(&lindblom_dudh_and_dvdh, lindblom_u_and_v,
                            lindblom_enthalpy, equation_of_state);
      },
      u_and_v, central_log_enthalpy, final_log_enthalpy, initial_step,
      std::ref(observer));
  outer_radius_ = observer.radius.back();
  mass_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.mass, 5);
  // log_enthalpy(radius) is almost linear so an interpolant of order 3
  // maximizes precision
  log_enthalpy_interpolant_ =
      intrp::BarycentricRational(observer.radius, observer.log_enthalpy, 3);
}

double TovSolution::outer_radius() const noexcept { return outer_radius_; }

double TovSolution::mass(const double r) const noexcept {
  return mass_interpolant_(r);
}

Scalar<DataVector> TovSolution::mass(const Scalar<DataVector>& radius) const
    noexcept {
  DataVector mass(radius.size(), 0.0);
  for (size_t i = 0; i < radius.size(); i++) {
    mass[i] = mass_interpolant_(get(radius)[i]);
  }
  return Scalar<DataVector>{std::move(mass)};
}

double TovSolution::log_specific_enthalpy(const double r) const noexcept {
  return log_enthalpy_interpolant_(r);
}

Scalar<DataVector> TovSolution::log_specific_enthalpy(
    const Scalar<DataVector>& radius) const noexcept {
  DataVector log_specific_enthalpy(radius.size(), 0.0);
  for (size_t i = 0; i < radius.size(); i++) {
    log_specific_enthalpy[i] = log_enthalpy_interpolant_(get(radius)[i]);
  }
  return Scalar<DataVector>{std::move(log_specific_enthalpy)};
}

double TovSolution::specific_enthalpy(const double r) const noexcept {
  return std::exp(log_enthalpy_interpolant_(r));
}

Scalar<DataVector> TovSolution::specific_enthalpy(
    const Scalar<DataVector>& radius) const noexcept {
  return Scalar<DataVector>{
      exp(get(TovSolution::log_specific_enthalpy(radius)))};
}

void TovSolution::pup(PUP::er& p) noexcept {  // NOLINT
  p | outer_radius_;
  p | mass_interpolant_;
  p | log_enthalpy_interpolant_;
}
}  // namespace Solutions
}  // namespace gr
/// \endcond


/* boost random/subtract_with_carry.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: subtract_with_carry.hpp 72951 2011-07-07 04:57:37Z steven_watanabe $
 *
 * Revision history
 *  2002-03-02  created
 */

#ifndef BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP
#define BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP

#include <boost/config/no_tr1/cmath.hpp>         // std::pow
#include <iostream>
#include <algorithm>     // std::equal
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/integer/integer_mask.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/seed_impl.hpp>
#include <boost/random/detail/generator_seed_seq.hpp>
#include <boost/random/linear_congruential.hpp>

namespace boost {
namespace random {

namespace detail {

struct subtract_with_carry_discard
{
    template<class Engine>
    static void apply(Engine& eng, boost::uintmax_t z)
    {
        typedef typename Engine::result_type IntType;
        const std::size_t short_lag = Engine::short_lag;
        const std::size_t long_lag = Engine::long_lag;
        std::size_t k = eng.k;
        IntType carry = eng.carry;
        if(k != 0) {
            // increment k until it becomes 0.
            if(k < short_lag) {
                std::size_t limit = (short_lag - k) < z?
                    short_lag : (k + static_cast<std::size_t>(z));
                for(std::size_t j = k; j < limit; ++j) {
                    carry = eng.do_update(j, j + long_lag - short_lag, carry);
                }
            }
            std::size_t limit = (long_lag - k) < z?
                long_lag : (k + static_cast<std::size_t>(z));
            std::size_t start = (k < short_lag ? short_lag : k);
            for(std::size_t j = start; j < limit; ++j) {
                carry = eng.do_update(j, j - short_lag, carry);
            }
        }

        k = ((z % long_lag) + k) % long_lag;

        if(k < z) {
            // main loop: update full blocks from k = 0 to long_lag
            for(std::size_t i = 0; i < (z - k) / long_lag; ++i) {
                for(std::size_t j = 0; j < short_lag; ++j) {
                    carry = eng.do_update(j, j + long_lag - short_lag, carry);
                }
                for(std::size_t j = short_lag; j < long_lag; ++j) {
                    carry = eng.do_update(j, j - short_lag, carry);
                }
            }

            // Update the last partial block
            std::size_t limit = short_lag < k? short_lag : k;
            for(std::size_t j = 0; j < limit; ++j) {
                carry = eng.do_update(j, j + long_lag - short_lag, carry);
            }
            for(std::size_t j = short_lag; j < k; ++j) {
                carry = eng.do_update(j, j - short_lag, carry);
            }
        }
        eng.carry = carry;
        eng.k = k;
    }
};

}

/**
 * Instantiations of @c subtract_with_carry_engine model a
 * \pseudo_random_number_generator.  The algorithm is
 * described in
 *
 *  @blockquote
 *  "A New Class of Random Number Generators", George
 *  Marsaglia and Arif Zaman, Annals of Applied Probability,
 *  Volume 1, Number 3 (1991), 462-480.
 *  @endblockquote
 */
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
class subtract_with_carry_engine
{
public:
    typedef IntType result_type;
    BOOST_STATIC_CONSTANT(std::size_t, word_size = w);
    BOOST_STATIC_CONSTANT(std::size_t, long_lag = r);
    BOOST_STATIC_CONSTANT(std::size_t, short_lag = s);
    BOOST_STATIC_CONSTANT(uint32_t, default_seed = 19780503u);

    // Required by the old Boost.Random concepts
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    // Backwards compatibility
    BOOST_STATIC_CONSTANT(result_type, modulus = (result_type(1) << w));

    BOOST_STATIC_ASSERT(std::numeric_limits<result_type>::is_integer);

    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with the default seed.
     */
    subtract_with_carry_engine() { seed(); }
    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with @c value.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry_engine,
                                               IntType, value)
    { seed(value); }
    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with values produced by @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(subtract_with_carry_engine,
                                             SeedSeq, seq)
    { seed(seq); }
    /**
     * Constructs a new @c subtract_with_carry_engine and seeds
     * it with values from a range.  first is updated to point
     * one past the last value consumed.  If there are not
     * enough elements in the range to fill the entire state of
     * the generator, throws @c std::invalid_argument.
     */
    template<class It> subtract_with_carry_engine(It& first, It last)
    { seed(first,last); }

    // compiler-generated copy ctor and assignment operator are fine

    /** Seeds the generator with the default seed. */
    void seed() { seed(default_seed); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry_engine,
                                        IntType, value)
    {
        typedef linear_congruential_engine<uint32_t,40014,0,2147483563> gen_t;
        gen_t intgen(static_cast<boost::uint32_t>(value));
        detail::generator_seed_seq<gen_t> gen(intgen);
        seed(gen);
    }

    /** Seeds the generator with values produced by @c seq.generate(). */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(subtract_with_carry, SeedSeq, seq)
    {
        detail::seed_array_int<w>(seq, x);
        carry = (x[long_lag-1] == 0);
        k = 0;
    }

    /**
     * Seeds the generator with values from a range.  Updates @c first to
     * point one past the last consumed value.  If the range does not
     * contain enough elements to fill the entire state of the generator,
     * throws @c std::invalid_argument.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_int<w>(first, last, x);
        carry = (x[long_lag-1] == 0);
        k = 0;
    }

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return 0; }
    /** Returns the largest value that the generator can produce. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return boost::low_bits_mask_t<w>::sig_bits; }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        std::size_t short_index =
            (k < short_lag)?
                (k + long_lag - short_lag) :
                (k - short_lag);
        carry = do_update(k, short_index, carry);
        IntType result = x[k];
        ++k;
        if(k >= long_lag)
            k = 0;
        return result;
    }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        detail::subtract_with_carry_discard::apply(*this, z);
    }

    /** Fills a range with random values. */
    template<class It>
    void generate(It first, It last)
    { detail::generate_from_int(*this, first, last); }

    /** Writes a @c subtract_with_carry_engine to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, subtract_with_carry_engine, f)
    {
        for(unsigned int j = 0; j < f.long_lag; ++j)
            os << f.compute(j) << ' ';
        os << f.carry;
        return os;
    }

    /** Reads a @c subtract_with_carry_engine from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, subtract_with_carry_engine, f)
    {
        for(unsigned int j = 0; j < f.long_lag; ++j)
            is >> f.x[j] >> std::ws;
        is >> f.carry;
        f.k = 0;
        return is;
    }

    /**
     * Returns true if the two generators will produce identical
     * sequences of values.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(subtract_with_carry_engine, x, y)
    {
        for(unsigned int j = 0; j < r; ++j)
            if(x.compute(j) != y.compute(j))
                return false;
        return true;
    }

    /**
     * Returns true if the two generators will produce different
     * sequences of values.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(subtract_with_carry_engine)

private:
    /// \cond show_private
    // returns x(i-r+index), where index is in 0..r-1
    IntType compute(unsigned int index) const
    {
        return x[(k+index) % long_lag];
    }

    friend struct detail::subtract_with_carry_discard;

    IntType do_update(std::size_t current, std::size_t short_index, IntType carry)
    {
        IntType delta;
        IntType temp = x[current] + carry;
        if (x[short_index] >= temp) {
            // x(n) >= 0
            delta =  x[short_index] - temp;
            carry = 0;
        } else {
            // x(n) < 0
            delta = modulus - temp + x[short_index];
            carry = 1;
        }
        x[current] = delta;
        return carry;
    }
    /// \endcond

    // state representation; next output (state) is x(i)
    //   x[0]  ... x[k] x[k+1] ... x[long_lag-1]     represents
    //  x(i-k) ... x(i) x(i+1) ... x(i-k+long_lag-1)
    // speed: base: 20-25 nsec
    // ranlux_4: 230 nsec, ranlux_7: 430 nsec, ranlux_14: 810 nsec
    // This state representation makes operator== and save/restore more
    // difficult, because we've already computed "too much" and thus
    // have to undo some steps to get at x(i-r) etc.

    // state representation: next output (state) is x(i)
    //   x[0]  ... x[k] x[k+1]          ... x[long_lag-1]     represents
    //  x(i-k) ... x(i) x(i-long_lag+1) ... x(i-k-1)
    // speed: base 28 nsec
    // ranlux_4: 370 nsec, ranlux_7: 688 nsec, ranlux_14: 1343 nsec
    IntType x[long_lag];
    std::size_t k;
    IntType carry;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const bool subtract_with_carry_engine<IntType, w, s, r>::has_fixed_range;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const IntType subtract_with_carry_engine<IntType, w, s, r>::modulus;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_engine<IntType, w, s, r>::word_size;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_engine<IntType, w, s, r>::long_lag;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_engine<IntType, w, s, r>::short_lag;
template<class IntType, std::size_t w, std::size_t s, std::size_t r>
const uint32_t subtract_with_carry_engine<IntType, w, s, r>::default_seed;
#endif

// use a floating-point representation to produce values in [0..1)
/**
 * Instantiations of \subtract_with_carry_01_engine model a
 * \pseudo_random_number_generator.  The algorithm is
 * described in
 *
 *  @blockquote
 *  "A New Class of Random Number Generators", George
 *  Marsaglia and Arif Zaman, Annals of Applied Probability,
 *  Volume 1, Number 3 (1991), 462-480.
 *  @endblockquote
 */
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
class subtract_with_carry_01_engine
{
public:
    typedef RealType result_type;
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    BOOST_STATIC_CONSTANT(std::size_t, word_size = w);
    BOOST_STATIC_CONSTANT(std::size_t, long_lag = r);
    BOOST_STATIC_CONSTANT(std::size_t, short_lag = s);
    BOOST_STATIC_CONSTANT(boost::uint32_t, default_seed = 19780503u);

    BOOST_STATIC_ASSERT(!std::numeric_limits<result_type>::is_integer);

    /** Creates a new \subtract_with_carry_01_engine using the default seed. */
    subtract_with_carry_01_engine() { init_modulus(); seed(); }
    /** Creates a new subtract_with_carry_01_engine and seeds it with value. */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry_01_engine,
                                               boost::uint32_t, value)
    { init_modulus(); seed(value); }
    /**
     * Creates a new \subtract_with_carry_01_engine and seeds with with values
     * produced by seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(subtract_with_carry_01_engine,
                                             SeedSeq, seq)
    { init_modulus(); seed(seq); }
    /**
     * Creates a new \subtract_with_carry_01_engine and seeds it with values
     * from a range.  Advances first to point one past the last consumed
     * value.  If the range does not contain enough elements to fill the
     * entire state, throws @c std::invalid_argument.
     */
    template<class It> subtract_with_carry_01_engine(It& first, It last)
    { init_modulus(); seed(first,last); }

private:
    /// \cond show_private
    void init_modulus()
    {
#ifndef BOOST_NO_STDC_NAMESPACE
        // allow for Koenig lookup
        using std::pow;
#endif
        _modulus = pow(RealType(2), RealType(word_size));
    }
    /// \endcond

public:
    // compiler-generated copy ctor and assignment operator are fine

    /** Seeds the generator with the default seed. */
    void seed() { seed(default_seed); }

    /** Seeds the generator with @c value. */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry_01_engine,
                                        boost::uint32_t, value)
    {
        typedef linear_congruential_engine<uint32_t, 40014, 0, 2147483563> gen_t;
        gen_t intgen(value);
        detail::generator_seed_seq<gen_t> gen(intgen);
        seed(gen);
    }

    /** Seeds the generator with values produced by @c seq.generate(). */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(subtract_with_carry_01_engine,
                                      SeedSeq, seq)
    {
        detail::seed_array_real<w>(seq, x);
        carry = (x[long_lag-1] ? 0 : 1 / _modulus);
        k = 0;
    }

    /**
     * Seeds the generator with values from a range.  Updates first to
     * point one past the last consumed element.  If there are not
     * enough elements in the range to fill the entire state, throws
     * @c std::invalid_argument.
     */
    template<class It>
    void seed(It& first, It last)
    {
        detail::fill_array_real<w>(first, last, x);
        carry = (x[long_lag-1] ? 0 : 1 / _modulus);
        k = 0;
    }

    /** Returns the smallest value that the generator can produce. */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return result_type(0); }
    /** Returns the largest value that the generator can produce. */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return result_type(1); }

    /** Returns the next value of the generator. */
    result_type operator()()
    {
        std::size_t short_index =
            (k < short_lag) ?
                (k + long_lag - short_lag) :
                (k - short_lag);
        carry = do_update(k, short_index, carry);
        RealType result = x[k];
        ++k;
        if(k >= long_lag)
            k = 0;
        return result;
    }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    { detail::subtract_with_carry_discard::apply(*this, z); }

    /** Fills a range with random values. */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_real(*this, first, last); }

    /** Writes a \subtract_with_carry_01_engine to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, subtract_with_carry_01_engine, f)
    {
        std::ios_base::fmtflags oldflags =
            os.flags(os.dec | os.fixed | os.left);
        for(unsigned int j = 0; j < f.long_lag; ++j)
            os << (f.compute(j) * f._modulus) << ' ';
        os << (f.carry * f._modulus);
        os.flags(oldflags);
        return os;
    }

    /** Reads a \subtract_with_carry_01_engine from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, subtract_with_carry_01_engine, f)
    {
        RealType value;
        for(unsigned int j = 0; j < long_lag; ++j) {
            is >> value >> std::ws;
            f.x[j] = value / f._modulus;
        }
        is >> value;
        f.carry = value / f._modulus;
        f.k = 0;
        return is;
    }

    /** Returns true if the two generators will produce identical sequences. */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(subtract_with_carry_01_engine, x, y)
    {
        for(unsigned int j = 0; j < r; ++j)
            if(x.compute(j) != y.compute(j))
                return false;
        return true;
    }

    /** Returns true if the two generators will produce different sequences. */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(subtract_with_carry_01_engine)

private:
    /// \cond show_private
    RealType compute(unsigned int index) const
    {
        return x[(k+index) % long_lag];
    }

    friend struct detail::subtract_with_carry_discard;

    RealType do_update(std::size_t current, std::size_t short_index, RealType carry)
    {
        RealType delta = x[short_index] - x[current] - carry;
        if(delta < 0) {
            delta += RealType(1);
            carry = RealType(1)/_modulus;
        } else {
            carry = 0;
        }
        x[current] = delta;
        return carry;
    }
    /// \endcond
    std::size_t k;
    RealType carry;
    RealType x[long_lag];
    RealType _modulus;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const bool subtract_with_carry_01_engine<RealType, w, s, r>::has_fixed_range;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_01_engine<RealType, w, s, r>::word_size;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_01_engine<RealType, w, s, r>::long_lag;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const std::size_t subtract_with_carry_01_engine<RealType, w, s, r>::short_lag;
template<class RealType, std::size_t w, std::size_t s, std::size_t r>
const uint32_t subtract_with_carry_01_engine<RealType, w, s, r>::default_seed;
#endif

/// \cond show_deprecated

template<class IntType, IntType m, unsigned s, unsigned r, IntType v>
class subtract_with_carry :
    public subtract_with_carry_engine<IntType,
        boost::static_log2<m>::value, s, r>
{
    typedef subtract_with_carry_engine<IntType,
        boost::static_log2<m>::value, s, r> base_type;
public:
    subtract_with_carry() {}
    BOOST_RANDOM_DETAIL_GENERATOR_CONSTRUCTOR(subtract_with_carry, Gen, gen)
    { seed(gen); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry,
                                               IntType, val)
    { seed(val); }
    template<class It>
    subtract_with_carry(It& first, It last) : base_type(first, last) {}
    void seed() { base_type::seed(); }
    BOOST_RANDOM_DETAIL_GENERATOR_SEED(subtract_with_carry, Gen, gen)
    {
        detail::generator_seed_seq<Gen> seq(gen);
        base_type::seed(seq);
    }
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry, IntType, val)
    { base_type::seed(val); }
    template<class It>
    void seed(It& first, It last) { base_type::seed(first, last); }
};

template<class RealType, int w, unsigned s, unsigned r, int v = 0>
class subtract_with_carry_01 :
    public subtract_with_carry_01_engine<RealType, w, s, r>
{
    typedef subtract_with_carry_01_engine<RealType, w, s, r> base_type;
public:
    subtract_with_carry_01() {}
    BOOST_RANDOM_DETAIL_GENERATOR_CONSTRUCTOR(subtract_with_carry_01, Gen, gen)
    { seed(gen); }
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(subtract_with_carry_01,
                                               uint32_t, val)
    { seed(val); }
    template<class It>
    subtract_with_carry_01(It& first, It last) : base_type(first, last) {}
    void seed() { base_type::seed(); }
    BOOST_RANDOM_DETAIL_GENERATOR_SEED(subtract_with_carry_01, Gen, gen)
    {
        detail::generator_seed_seq<Gen> seq(gen);
        base_type::seed(seq);
    }
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(subtract_with_carry_01, uint32_t, val)
    { base_type::seed(val); }
    template<class It>
    void seed(It& first, It last) { base_type::seed(first, last); }
};

/// \endcond

namespace detail {

template<class Engine>
struct generator_bits;

template<class RealType, std::size_t w, std::size_t s, std::size_t r>
struct generator_bits<subtract_with_carry_01_engine<RealType, w, s, r> > {
    static std::size_t value() { return w; }
};

template<class RealType, int w, unsigned s, unsigned r, int v>
struct generator_bits<subtract_with_carry_01<RealType, w, s, r, v> > {
    static std::size_t value() { return w; }
};

}

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_SUBTRACT_WITH_CARRY_HPP


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_VECTOR_ASSIGN_
#define _BOOST_UBLAS_VECTOR_ASSIGN_

#include <boost/numeric/ublas/functional.hpp> // scalar_assign
// Required for make_conformant storage
#include <vector>

// Iterators based on ideas of Jeremy Siek

namespace boost { namespace numeric { namespace ublas {
namespace detail {

    // Weak equality check - useful to compare equality two arbitary vector expression results.
    // Since the actual expressions are unknown, we check for and arbitary error bound
    // on the relative error.
    // For a linear expression the infinity norm makes sense as we do not know how the elements will be
    // combined in the expression. False positive results are inevitable for arbirary expressions!
    template<class E1, class E2, class S>
    BOOST_UBLAS_INLINE
    bool equals (const vector_expression<E1> &e1, const vector_expression<E2> &e2, S epsilon, S min_norm) {
        return norm_inf (e1 - e2) < epsilon *
               std::max<S> (std::max<S> (norm_inf (e1), norm_inf (e2)), min_norm);
    }

    template<class E1, class E2>
    BOOST_UBLAS_INLINE
    bool expression_type_check (const vector_expression<E1> &e1, const vector_expression<E2> &e2) {
        typedef typename type_traits<typename promote_traits<typename E1::value_type,
                                     typename E2::value_type>::promote_type>::real_type real_type;
        return equals (e1, e2, BOOST_UBLAS_TYPE_CHECK_EPSILON, BOOST_UBLAS_TYPE_CHECK_MIN);
    }

    // Make sparse proxies conformant
    template<class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void make_conformant (V &v, const vector_expression<E> &e) {
        BOOST_UBLAS_CHECK (v.size () == e ().size (), bad_size ());
        typedef typename V::size_type size_type;
        typedef typename V::difference_type difference_type;
        typedef typename V::value_type value_type;
        // FIXME unbounded_array with push_back maybe better
        std::vector<size_type> index;
        typename V::iterator it (v.begin ());
        typename V::iterator it_end (v.end ());
        typename E::const_iterator ite (e ().begin ());
        typename E::const_iterator ite_end (e ().end ());
        if (it != it_end && ite != ite_end) {
            size_type it_index = it.index (), ite_index = ite.index ();
            while (true) {
                difference_type compare = it_index - ite_index;
                if (compare == 0) {
                    ++ it, ++ ite;
                    if (it != it_end && ite != ite_end) {
                        it_index = it.index ();
                        ite_index = ite.index ();
                    } else
                        break;
                } else if (compare < 0) {
                    increment (it, it_end, - compare);
                    if (it != it_end)
                        it_index = it.index ();
                    else
                        break;
                } else if (compare > 0) {
                    if (*ite != value_type/*zero*/())
                        index.push_back (ite.index ());
                    ++ ite;
                    if (ite != ite_end)
                        ite_index = ite.index ();
                    else
                        break;
                }
            }
        }

        while (ite != ite_end) {
            if (*ite != value_type/*zero*/())
                index.push_back (ite.index ());
            ++ ite;
        }
        for (size_type k = 0; k < index.size (); ++ k)
            v (index [k]) = value_type/*zero*/();
    }

}//namespace detail

    // Explicitly iterating
    template<template <class T1, class T2> class F, class V, class T>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void iterating_vector_assign_scalar (V &v, const T &t) {
        typedef F<typename V::iterator::reference, T> functor_type;
        typedef typename V::difference_type difference_type;
        difference_type size (v.size ());
        typename V::iterator it (v.begin ());
        BOOST_UBLAS_CHECK (v.end () - it == size, bad_size ());
#ifndef BOOST_UBLAS_USE_DUFF_DEVICE
        while (-- size >= 0)
            functor_type::apply (*it, t), ++ it;
#else
        DD (size, 4, r, (functor_type::apply (*it, t), ++ it));
#endif
    }
    // Explicitly case
    template<template <class T1, class T2> class F, class V, class T>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void indexing_vector_assign_scalar (V &v, const T &t) {
        typedef F<typename V::reference, T> functor_type;
        typedef typename V::size_type size_type;
        size_type size (v.size ());
#ifndef BOOST_UBLAS_USE_DUFF_DEVICE
        for (size_type i = 0; i < size; ++ i)
            functor_type::apply (v (i), t);
#else
        size_type i (0);
        DD (size, 4, r, (functor_type::apply (v (i), t), ++ i));
#endif
    }

    // Dense (proxy) case
    template<template <class T1, class T2> class F, class V, class T>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign_scalar (V &v, const T &t, dense_proxy_tag) {
#ifdef BOOST_UBLAS_USE_INDEXING
        indexing_vector_assign_scalar<F> (v, t);
#elif BOOST_UBLAS_USE_ITERATING
        iterating_vector_assign_scalar<F> (v, t);
#else
        typedef typename V::size_type size_type;
        size_type size (v.size ());
        if (size >= BOOST_UBLAS_ITERATOR_THRESHOLD)
            iterating_vector_assign_scalar<F> (v, t);
        else
            indexing_vector_assign_scalar<F> (v, t);
#endif
    }
    // Packed (proxy) case
    template<template <class T1, class T2> class F, class V, class T>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign_scalar (V &v, const T &t, packed_proxy_tag) {
        typedef F<typename V::iterator::reference, T> functor_type;
        typedef typename V::difference_type difference_type;
        typename V::iterator it (v.begin ());
        difference_type size (v.end () - it);
        while (-- size >= 0)
            functor_type::apply (*it, t), ++ it;
    }
    // Sparse (proxy) case
    template<template <class T1, class T2> class F, class V, class T>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign_scalar (V &v, const T &t, sparse_proxy_tag) {
        typedef F<typename V::iterator::reference, T> functor_type;
        typename V::iterator it (v.begin ());
        typename V::iterator it_end (v.end ());
        while (it != it_end)
            functor_type::apply (*it, t), ++ it;
    }

    // Dispatcher
    template<template <class T1, class T2> class F, class V, class T>
    BOOST_UBLAS_INLINE
    void vector_assign_scalar (V &v, const T &t) {
        typedef typename V::storage_category storage_category;
        vector_assign_scalar<F> (v, t, storage_category ());
    }

    template<class SC, bool COMPUTED, class RI>
    struct vector_assign_traits {
        typedef SC storage_category;
    };

    template<bool COMPUTED>
    struct vector_assign_traits<dense_tag, COMPUTED, packed_random_access_iterator_tag> {
        typedef packed_tag storage_category;
    };
    template<>
    struct vector_assign_traits<dense_tag, false, sparse_bidirectional_iterator_tag> {
        typedef sparse_tag storage_category;
    };
    template<>
    struct vector_assign_traits<dense_tag, true, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<bool COMPUTED>
    struct vector_assign_traits<dense_proxy_tag, COMPUTED, packed_random_access_iterator_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct vector_assign_traits<dense_proxy_tag, false, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct vector_assign_traits<dense_proxy_tag, true, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct vector_assign_traits<packed_tag, false, sparse_bidirectional_iterator_tag> {
        typedef sparse_tag storage_category;
    };
    template<>
    struct vector_assign_traits<packed_tag, true, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<bool COMPUTED>
    struct vector_assign_traits<packed_proxy_tag, COMPUTED, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct vector_assign_traits<sparse_tag, true, dense_random_access_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct vector_assign_traits<sparse_tag, true, packed_random_access_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct vector_assign_traits<sparse_tag, true, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    // Explicitly iterating
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void iterating_vector_assign (V &v, const vector_expression<E> &e) {
        typedef F<typename V::iterator::reference, typename E::value_type> functor_type;
        typedef typename V::difference_type difference_type;
        difference_type size (BOOST_UBLAS_SAME (v.size (), e ().size ()));
        typename V::iterator it (v.begin ());
        BOOST_UBLAS_CHECK (v.end () - it == size, bad_size ());
        typename E::const_iterator ite (e ().begin ());
        BOOST_UBLAS_CHECK (e ().end () - ite == size, bad_size ());
#ifndef BOOST_UBLAS_USE_DUFF_DEVICE
        while (-- size >= 0)
            functor_type::apply (*it, *ite), ++ it, ++ ite;
#else
        DD (size, 2, r, (functor_type::apply (*it, *ite), ++ it, ++ ite));
#endif
    }
    // Explicitly indexing
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void indexing_vector_assign (V &v, const vector_expression<E> &e) {
        typedef F<typename V::reference, typename E::value_type> functor_type;
        typedef typename V::size_type size_type;
        size_type size (BOOST_UBLAS_SAME (v.size (), e ().size ()));
#ifndef BOOST_UBLAS_USE_DUFF_DEVICE
        for (size_type i = 0; i < size; ++ i)
            functor_type::apply (v (i), e () (i));
#else
        size_type i (0);
        DD (size, 2, r, (functor_type::apply (v (i), e () (i)), ++ i));
#endif
    }

    // Dense (proxy) case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign (V &v, const vector_expression<E> &e, dense_proxy_tag) {
#ifdef BOOST_UBLAS_USE_INDEXING
        indexing_vector_assign<F> (v, e);
#elif BOOST_UBLAS_USE_ITERATING
        iterating_vector_assign<F> (v, e);
#else
        typedef typename V::size_type size_type;
        size_type size (BOOST_UBLAS_SAME (v.size (), e ().size ()));
        if (size >= BOOST_UBLAS_ITERATOR_THRESHOLD)
            iterating_vector_assign<F> (v, e);
        else
            indexing_vector_assign<F> (v, e);
#endif
    }
    // Packed (proxy) case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign (V &v, const vector_expression<E> &e, packed_proxy_tag) {
        BOOST_UBLAS_CHECK (v.size () == e ().size (), bad_size ());
        typedef F<typename V::iterator::reference, typename E::value_type> functor_type;
        typedef typename V::difference_type difference_type;
        typedef typename V::value_type value_type;
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        indexing_vector_assign<scalar_assign> (cv, v);
        indexing_vector_assign<F> (cv, e);
#endif
        typename V::iterator it (v.begin ());
        typename V::iterator it_end (v.end ());
        typename E::const_iterator ite (e ().begin ());
        typename E::const_iterator ite_end (e ().end ());
        difference_type it_size (it_end - it);
        difference_type ite_size (ite_end - ite);
        if (it_size > 0 && ite_size > 0) {
            difference_type size ((std::min) (difference_type (it.index () - ite.index ()), ite_size));
            if (size > 0) {
                ite += size;
                ite_size -= size;
            }
        }
        if (it_size > 0 && ite_size > 0) {
            difference_type size ((std::min) (difference_type (ite.index () - it.index ()), it_size));
            if (size > 0) {
                it_size -= size;
                if (!functor_type::computed) {
                    while (-- size >= 0)    // zeroing
                        functor_type::apply (*it, value_type/*zero*/()), ++ it;
                } else {
                    it += size;
                }
            }
        }
        difference_type size ((std::min) (it_size, ite_size));
        it_size -= size;
        ite_size -= size;
        while (-- size >= 0)
            functor_type::apply (*it, *ite), ++ it, ++ ite;
        size = it_size;
        if (!functor_type::computed) {
            while (-- size >= 0)    // zeroing
                functor_type::apply (*it, value_type/*zero*/()), ++ it;
        } else {
            it += size;
        }
#if BOOST_UBLAS_TYPE_CHECK
        if (! disable_type_check<bool>::value)
            BOOST_UBLAS_CHECK (detail::expression_type_check (v, cv),
                               external_logic ("external logic or bad condition of inputs"));
#endif
    }
    // Sparse case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign (V &v, const vector_expression<E> &e, sparse_tag) {
        BOOST_UBLAS_CHECK (v.size () == e ().size (), bad_size ());
        typedef F<typename V::iterator::reference, typename E::value_type> functor_type;
        BOOST_STATIC_ASSERT ((!functor_type::computed));
        typedef typename V::value_type value_type;
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        indexing_vector_assign<scalar_assign> (cv, v);
        indexing_vector_assign<F> (cv, e);
#endif
        v.clear ();
        typename E::const_iterator ite (e ().begin ());
        typename E::const_iterator ite_end (e ().end ());
        while (ite != ite_end) {
            value_type t (*ite);
            if (t != value_type/*zero*/())
                v.insert_element (ite.index (), t);
            ++ ite;
        }
#if BOOST_UBLAS_TYPE_CHECK
        if (! disable_type_check<bool>::value)
            BOOST_UBLAS_CHECK (detail::expression_type_check (v, cv),
                               external_logic ("external logic or bad condition of inputs"));
#endif
    }
    // Sparse proxy or functional case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_assign (V &v, const vector_expression<E> &e, sparse_proxy_tag) {
        BOOST_UBLAS_CHECK (v.size () == e ().size (), bad_size ());
        typedef F<typename V::iterator::reference, typename E::value_type> functor_type;
        typedef typename V::size_type size_type;
        typedef typename V::difference_type difference_type;
        typedef typename V::value_type value_type;
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        indexing_vector_assign<scalar_assign> (cv, v);
        indexing_vector_assign<F> (cv, e);
#endif
        detail::make_conformant (v, e);

        typename V::iterator it (v.begin ());
        typename V::iterator it_end (v.end ());
        typename E::const_iterator ite (e ().begin ());
        typename E::const_iterator ite_end (e ().end ());
        if (it != it_end && ite != ite_end) {
            size_type it_index = it.index (), ite_index = ite.index ();
            while (true) {
                difference_type compare = it_index - ite_index;
                if (compare == 0) {
                    functor_type::apply (*it, *ite);
                    ++ it, ++ ite;
                    if (it != it_end && ite != ite_end) {
                        it_index = it.index ();
                        ite_index = ite.index ();
                    } else
                        break;
                } else if (compare < 0) {
                    if (!functor_type::computed) {
                        functor_type::apply (*it, value_type/*zero*/());
                        ++ it;
                    } else
                        increment (it, it_end, - compare);
                    if (it != it_end)
                        it_index = it.index ();
                    else
                        break;
                } else if (compare > 0) {
                    increment (ite, ite_end, compare);
                    if (ite != ite_end)
                        ite_index = ite.index ();
                    else
                        break;
                }
            }
        }

        if (!functor_type::computed) {
            while (it != it_end) {  // zeroing
                functor_type::apply (*it, value_type/*zero*/());
                ++ it;
            }
        } else {
            it = it_end;
        }
#if BOOST_UBLAS_TYPE_CHECK
        if (! disable_type_check<bool>::value)
            BOOST_UBLAS_CHECK (detail::expression_type_check (v, cv),
                               external_logic ("external logic or bad condition of inputs"));
#endif
    }

    // Dispatcher
    template<template <class T1, class T2> class F, class V, class E>
    BOOST_UBLAS_INLINE
    void vector_assign (V &v, const vector_expression<E> &e) {
        typedef typename vector_assign_traits<typename V::storage_category,
                                              F<typename V::reference, typename E::value_type>::computed,
                                              typename E::const_iterator::iterator_category>::storage_category storage_category;
        vector_assign<F> (v, e, storage_category ());
    }

    template<class SC, class RI>
    struct vector_swap_traits {
        typedef SC storage_category;
    };

    template<>
    struct vector_swap_traits<dense_proxy_tag, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct vector_swap_traits<packed_proxy_tag, sparse_bidirectional_iterator_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    // Dense (proxy) case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_swap (V &v, vector_expression<E> &e, dense_proxy_tag) {
        typedef F<typename V::iterator::reference, typename E::iterator::reference> functor_type;
        typedef typename V::difference_type difference_type;
        difference_type size (BOOST_UBLAS_SAME (v.size (), e ().size ()));
        typename V::iterator it (v.begin ());
        typename E::iterator ite (e ().begin ());
        while (-- size >= 0)
            functor_type::apply (*it, *ite), ++ it, ++ ite;
    }
    // Packed (proxy) case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_swap (V &v, vector_expression<E> &e, packed_proxy_tag) {
        typedef F<typename V::iterator::reference, typename E::iterator::reference> functor_type;
        typedef typename V::difference_type difference_type;
        typename V::iterator it (v.begin ());
        typename V::iterator it_end (v.end ());
        typename E::iterator ite (e ().begin ());
        typename E::iterator ite_end (e ().end ());
        difference_type it_size (it_end - it);
        difference_type ite_size (ite_end - ite);
        if (it_size > 0 && ite_size > 0) {
            difference_type size ((std::min) (difference_type (it.index () - ite.index ()), ite_size));
            if (size > 0) {
                ite += size;
                ite_size -= size;
            }
        }
        if (it_size > 0 && ite_size > 0) {
            difference_type size ((std::min) (difference_type (ite.index () - it.index ()), it_size));
            if (size > 0)
                it_size -= size;
        }
        difference_type size ((std::min) (it_size, ite_size));
        it_size -= size;
        ite_size -= size;
        while (-- size >= 0)
            functor_type::apply (*it, *ite), ++ it, ++ ite;
    }
    // Sparse proxy case
    template<template <class T1, class T2> class F, class V, class E>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    void vector_swap (V &v, vector_expression<E> &e, sparse_proxy_tag) {
        BOOST_UBLAS_CHECK (v.size () == e ().size (), bad_size ());
        typedef F<typename V::iterator::reference, typename E::iterator::reference> functor_type;
        typedef typename V::size_type size_type;
        typedef typename V::difference_type difference_type;

        detail::make_conformant (v, e);
        // FIXME should be a seperate restriction for E
        detail::make_conformant (e (), v);

        typename V::iterator it (v.begin ());
        typename V::iterator it_end (v.end ());
        typename E::iterator ite (e ().begin ());
        typename E::iterator ite_end (e ().end ());
        if (it != it_end && ite != ite_end) {
            size_type it_index = it.index (), ite_index = ite.index ();
            while (true) {
                difference_type compare = it_index - ite_index;
                if (compare == 0) {
                    functor_type::apply (*it, *ite);
                    ++ it, ++ ite;
                    if (it != it_end && ite != ite_end) {
                        it_index = it.index ();
                        ite_index = ite.index ();
                    } else
                        break;
                } else if (compare < 0) {
                    increment (it, it_end, - compare);
                    if (it != it_end)
                        it_index = it.index ();
                    else
                        break;
                } else if (compare > 0) {
                    increment (ite, ite_end, compare);
                    if (ite != ite_end)
                        ite_index = ite.index ();
                    else
                        break;
                }
            }
        }

#if BOOST_UBLAS_TYPE_CHECK
        increment (ite, ite_end);
        increment (it, it_end);
#endif
    }

    // Dispatcher
    template<template <class T1, class T2> class F, class V, class E>
    BOOST_UBLAS_INLINE
    void vector_swap (V &v, vector_expression<E> &e) {
        typedef typename vector_swap_traits<typename V::storage_category,
                                            typename E::const_iterator::iterator_category>::storage_category storage_category;
        vector_swap<F> (v, e, storage_category ());
    }

}}}

#endif


#include <unordered_map>

#include <boost/filesystem.hpp>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

#include "GraphOptimizer/OptApp.h"
#include "GraphOptimizer/vertigo/vertex_switchLinear.h"
#include "GraphOptimizer/vertigo/edge_switchPrior.h"
#include "GraphOptimizer/vertigo/edge_se3Switchable.h"

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType>
    SlamLinearCSparseSolver;
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType>
    SlamLinearPCGSolver;
typedef std::unordered_map<int, g2o::HyperGraph::Vertex *> VertexIDMap;
typedef std::pair<int, g2o::HyperGraph::Vertex *> VertexIDPair;
typedef std::set<g2o::HyperGraph::Edge *> EdgeSet;

COptApp::COptApp(void) {}

COptApp::~COptApp(void) {}

bool COptApp::Init() {
    namespace fs = boost::filesystem;
    if (fs::exists(fs::path(odometry_log_file_))) {
        odometry_traj_.LoadFromFile(odometry_log_file_);
        if (fs::exists(fs::path(odometry_info_file_))) {
            odometry_info_.LoadFromFile(odometry_info_file_);
        }
    }
    if (fs::exists(fs::path(loop_log_file_))) {
        loop_traj_.LoadFromFile(loop_log_file_);
        if (fs::exists(fs::path(loop_info_file_))) {
            loop_info_.LoadFromFile(loop_info_file_);
        }
    }
    pose_traj_.data_.clear();
    pose_traj_.data_.push_back(
        FramedTransformation(0, 0, 1, Eigen::Matrix4d::Identity()));
    for (int i = 0; i < (int)odometry_traj_.data_.size(); i++) {
        pose_traj_.data_.push_back(
            FramedTransformation(i + 1, i + 1, i + 2,
                                 pose_traj_.data_[i].transformation_ *
                                     odometry_traj_.data_[i].transformation_));
    }
    return (odometry_traj_.data_.size() > 0);
}

void COptApp::OptimizeSwitchable() {
    struct SwitchableEdge {
      public:
        VertexSwitchLinear *v_;
        EdgeSwitchPrior *ep_;
        EdgeSE3Switchable *e_;
        FramedTransformation *t_;
    };

    g2o::SparseOptimizer *optimizer;
    optimizer = new g2o::SparseOptimizer();
    optimizer->setVerbose(true);
    SlamBlockSolver *solver = NULL;
    SlamLinearCSparseSolver *linearSolver = new SlamLinearCSparseSolver();
    linearSolver->setBlockOrdering(false);
    solver = new SlamBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *algo =
        new g2o::OptimizationAlgorithmLevenberg(solver);
    optimizer->setAlgorithm(algo);

    std::vector<SwitchableEdge> switch_edge;

    Eigen::Matrix<double, 6, 6> default_information;
    default_information = Eigen::Matrix<double, 6, 6>::Identity();

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId(i);
        v->setEstimate(Eigen2G2O(pose_traj_.data_[i].transformation_));
        if (i == 0) {
            v->setFixed(true);
        }
        optimizer->addVertex(v);

        if (i > 0) {
            g2o::EdgeSE3 *g2o_edge = new g2o::EdgeSE3();
            g2o_edge->vertices()[0] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i - 1));
            g2o_edge->vertices()[1] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
            g2o_edge->setMeasurement(g2o::internal::fromSE3Quat(
                Eigen2G2O(odometry_traj_.data_[i - 1].transformation_)));
            if (odometry_info_.data_.size() > 0) {
                g2o_edge->setInformation(
                    odometry_info_.data_[i - 1].information_);
            } else {
                g2o_edge->setInformation(default_information);
            }
            optimizer->addEdge(g2o_edge);
        }
    }

    for (int i = 0; i < (int)loop_traj_.data_.size(); i++) {
        FramedTransformation &t = loop_traj_.data_[i];

        SwitchableEdge edge;
        edge.t_ = &t;

        edge.v_ = new VertexSwitchLinear();
        edge.v_->setId(optimizer->vertices().size());
        edge.v_->setEstimate(1.0);
        optimizer->addVertex(edge.v_);

        edge.ep_ = new EdgeSwitchPrior();
        edge.ep_->vertices()[0] = edge.v_;
        edge.ep_->setMeasurement(1.0);
        edge.ep_->setInformation(Eigen::Matrix<double, 1, 1>::Identity() *
                                 weight_);
        optimizer->addEdge(edge.ep_);

        edge.e_ = new EdgeSE3Switchable();
        edge.e_->vertices()[0] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id1_));
        edge.e_->vertices()[1] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id2_));
        edge.e_->vertices()[2] = edge.v_;
        edge.e_->setMeasurement(
            g2o::internal::fromSE3Quat(Eigen2G2O(t.transformation_)));
        if (loop_info_.data_.size() > 0) {
            edge.e_->setInformation(loop_info_.data_[i].information_);
        } else {
            edge.e_->setInformation(default_information);
        }
        optimizer->addEdge(edge.e_);
        switch_edge.push_back(edge);
    }

    optimizer->initializeOptimization();
    optimizer->optimize(max_iteration_);

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
        pose_traj_.data_[i].transformation_ =
            G2O2Matrix4d(v->estimateAsSE3Quat());
    }
    pose_traj_.SaveToFile(pose_log_file_);

    loop_remain_traj_.data_.clear();
    for (int i = 0; i < (int)switch_edge.size(); i++) {
        SwitchableEdge &edge = switch_edge[i];
        if (edge.v_->estimate() > 0.5) {
            loop_remain_traj_.data_.push_back(loop_traj_.data_[i]);
        }
    }
    loop_remain_traj_.SaveToFile(loop_remain_log_file_);

    refine_traj_.data_.clear();
    for (int i = 0; i < (int)odometry_traj_.data_.size(); i++) {
        refine_traj_.data_.push_back(odometry_traj_.data_[i]);
    }
    for (int i = 0; i < (int)switch_edge.size(); i++) {
        SwitchableEdge &edge = switch_edge[i];
        if (edge.v_->estimate() > 0.5 &&
            loop_traj_.data_[i].id1_ + 1 < loop_traj_.data_[i].id2_) {
            refine_traj_.data_.push_back(loop_traj_.data_[i]);
        }
    }
    refine_traj_.SaveToFile(refine_log_file_);
}

void COptApp::OptimizeEM() {
    struct SwitchableEdge {
      public:
        double weight_;
        g2o::EdgeSE3 *e_;
        FramedTransformation *t_;
    };

    g2o::SparseOptimizer *optimizer;
    optimizer = new g2o::SparseOptimizer();
    optimizer->setVerbose(true);
    SlamBlockSolver *solver = NULL;
    SlamLinearCSparseSolver *linearSolver = new SlamLinearCSparseSolver();
    linearSolver->setBlockOrdering(false);
    solver = new SlamBlockSolver(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *algo =
        new g2o::OptimizationAlgorithmLevenberg(solver);
    optimizer->setAlgorithm(algo);

    std::vector<SwitchableEdge> switch_edge;

    Eigen::Matrix<double, 6, 6> default_information;
    default_information = Eigen::Matrix<double, 6, 6>::Identity();

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v = new g2o::VertexSE3();
        v->setId(i);
        v->setEstimate(Eigen2G2O(pose_traj_.data_[i].transformation_));
        if (i == 0) {
            v->setFixed(true);
        }
        optimizer->addVertex(v);

        if (i > 0) {
            g2o::EdgeSE3 *g2o_edge = new g2o::EdgeSE3();
            g2o_edge->vertices()[0] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i - 1));
            g2o_edge->vertices()[1] =
                dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
            g2o_edge->setMeasurement(g2o::internal::fromSE3Quat(
                Eigen2G2O(odometry_traj_.data_[i - 1].transformation_)));
            if (odometry_info_.data_.size() > 0) {
                g2o_edge->setInformation(
                    odometry_info_.data_[i - 1].information_);
            } else {
                g2o_edge->setInformation(default_information);
            }
            optimizer->addEdge(g2o_edge);
        }
    }

    for (int i = 0; i < (int)loop_traj_.data_.size(); i++) {
        FramedTransformation &t = loop_traj_.data_[i];
        SwitchableEdge edge;
        edge.t_ = &t;
        edge.weight_ = 0.0;

        edge.e_ = new g2o::EdgeSE3();
        edge.e_->vertices()[0] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id1_));
        edge.e_->vertices()[1] =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(t.id2_));
        edge.e_->setMeasurement(
            g2o::internal::fromSE3Quat(Eigen2G2O(t.transformation_)));
        if (loop_info_.data_.size() > 0) {
            edge.e_->setInformation(loop_info_.data_[i].information_);
        } else {
            edge.e_->setInformation(default_information);
        }
        optimizer->addEdge(edge.e_);
        switch_edge.push_back(edge);
    }

    for (int itr = 0; itr < max_iteration_; itr++) {
        // E step
        for (int i = 0; i < (int)switch_edge.size(); i++) {
            SwitchableEdge &edge = switch_edge[i];
            if (loop_info_.data_.size() > 0) {
                edge.e_->setInformation(loop_info_.data_[i].information_);
            } else {
                edge.e_->setInformation(default_information);
            }
            edge.e_->computeError();
            edge.weight_ = (weight_ * weight_) /
                           (weight_ * weight_ + switch_edge[i].e_->chi2());

            if (loop_info_.data_.size() > 0) {
                edge.e_->setInformation(loop_info_.data_[i].information_ *
                                        sqrt(edge.weight_));
            } else {
                edge.e_->setInformation(default_information *
                                        sqrt(edge.weight_));
            }
        }

        // M step
        optimizer->initializeOptimization();
        optimizer->optimize(1);
    }

    for (int i = 0; i < (int)pose_traj_.data_.size(); i++) {
        g2o::VertexSE3 *v =
            dynamic_cast<g2o::VertexSE3 *>(optimizer->vertex(i));
        pose_traj_.data_[i].transformation_ =
            G2O2Matrix4d(v->estimateAsSE3Quat());
    }
    pose_traj_.SaveToFile(pose_log_file_);

    loop_remain_traj_.data_.clear();
    for (int i = 0; i < (int)switch_edge.size(); i++) {
        SwitchableEdge &edge = switch_edge[i];
        if (edge.weight_ > 0.25) {
            loop_remain_traj_.data_.push_back(loop_traj_.data_[i]);
        }
    }
    loop_remain_traj_.SaveToFile(loop_remain_log_file_);
}


#include "gauss_newton_solver.h"
#include "prior_sparse_features.h"
#include "util.h"
#include "device_util.h"
#include "device_array.h"

#include <Eigen/Dense>
#include <chrono>

GaussNewtonSolver::GaussNewtonSolver()
	: m_face_bb(1)
	, m_sh_coefficients_gpu(9)
{
	cublasCreate(&m_cublas);
}

GaussNewtonSolver::~GaussNewtonSolver()
{
	cublasDestroy(m_cublas);
	destroyTextures();
}

void GaussNewtonSolver::solve(const std::vector<glm::vec2>& sparse_features, Face& face, cv::Mat& frame, glm::mat4& projection, const Pyramid& pyramid)
{
	if (sparse_features.empty()) //no tracking -> cublas doesnt like a getting matrix/vector of size 0
	{
		return;
	}

	auto number_of_levels = pyramid.getNumberOfLevels();
	if (sizeof(m_params.num_gn_iterations) / sizeof(int) != number_of_levels)
	{
		throw std::runtime_error("Please specify number of GN iteration per pyramid level!");
	}

	const int nFeatures = sparse_features.size();
	const int nShapeCoeffs = m_params.num_shape_coefficients;
	const int nExpressionCoeffs = m_params.num_expression_coefficients;
	const int nAlbedoCoeffs = m_params.num_albedo_coefficients;
	const int nFaceCoeffs = nShapeCoeffs + nExpressionCoeffs + nAlbedoCoeffs;
	const int nUnknowns = 7 + nFaceCoeffs + 9; //3+3+1 = 7 DoF for rotation, translation and intrinsics. Plus nFaceCoeffs for face parameters and 9 for lighting.

	const float wSparse = std::powf(10, m_params.sparse_weight_exponent);
	const float wDense = std::powf(10, m_params.dense_weight_exponent);
	const float wReg = std::powf(10, m_params.regularisation_weight_exponent);

	for (int pyramid_level = number_of_levels - 1; pyramid_level >= 0; pyramid_level--)
	{
		pyramid.setGraphicsSettings(pyramid_level, face.getGraphicsSettings());

		const int frameWidth = face.m_graphics_settings.texture_width;
		const int frameHeight = face.m_graphics_settings.texture_height;
		const int nPixels = frameWidth * frameHeight;
		const int nResiduals = 2 * nFeatures + 3 * nPixels + nFaceCoeffs; //nFaceCoeffs -> regularizer

		const auto& prior_local_ids = PriorSparseFeatures::get().getPriorIds();

		//TODO: Allocate all of the objects below once. So, move them out of here.
		auto jacobian_gpu = util::DeviceArray<float>(nResiduals * nUnknowns);
		auto residuals_gpu = util::DeviceArray<float>(nResiduals);
		auto result_gpu = util::DeviceArray<float>(nUnknowns);
		std::vector<float> result(nUnknowns);
		auto ids_gpu = util::DeviceArray<int>(prior_local_ids);
		auto key_pts_gpu = util::DeviceArray<glm::vec2>(sparse_features);

		cv::Mat processed_frame;
		cv::resize(frame, processed_frame, cv::Size(frameWidth, frameHeight));
		cv::cvtColor(processed_frame, processed_frame, cv::COLOR_BGR2RGB);
		util::DeviceArray<uchar> frame_gpu = util::DeviceArray<uchar>(3 * nPixels);
		util::copy(frame_gpu, processed_frame.data, 3 * nPixels);

		for (int iteration = 0; iteration < m_params.num_gn_iterations[pyramid_level]; ++iteration)
		{
			jacobian_gpu.memset(0);
			residuals_gpu.memset(0);
			face.computeFace();
			face.updateVertexBuffer();
			face.draw();

			auto face_pose = face.computeModelMatrix();
			Eigen::Matrix<float, 3, 3> jacobian_local;
			jacobian_local <<
				face_pose[0][0], face_pose[1][0], face_pose[2][0],
				face_pose[0][1], face_pose[1][1], face_pose[2][1],
				face_pose[0][2], face_pose[1][2], face_pose[2][2];

			glm::mat3 drx, dry, drz;
			face.computeRotationDerivatives(drx, dry, drz);

			mapRenderTargets(face);
			FaceBoundingBox face_bb = computeFaceBoundingBox(face.m_graphics_settings.texture_width, face.m_graphics_settings.texture_height);

			int n_current_residuals = 2 * nFeatures + nFaceCoeffs + 3 * face_bb.width * face_bb.height;

			util::copy(m_sh_coefficients_gpu, face.m_sh_coefficients, 9);

			//CUDA
			computeJacobian(
				//shared memory
				face_bb,
				nFeatures, frameWidth, frameHeight,
				nShapeCoeffs, nExpressionCoeffs, nAlbedoCoeffs, nUnknowns, n_current_residuals,
				face.m_number_of_vertices * 3,
				face.m_shape_coefficients.size(),
				face.m_expression_coefficients.size(),
				face.m_albedo_coefficients.size(),
				face.m_sh_coefficients.size(),
				wSparse, wDense, wReg,

				frame_gpu.getPtr(),

				face_pose, drx, dry, drz, projection, jacobian_local,

				//device memory input
				ids_gpu.getPtr(), face.m_current_face_gpu.getPtr(), key_pts_gpu.getPtr(),

				face.m_shape_basis_gpu.getPtr(),
				face.m_expression_basis_gpu.getPtr(),
				face.m_albedo_basis_gpu.getPtr(),

				face.m_shape_coefficients_gpu.getPtr(),
				face.m_expression_coefficients_gpu.getPtr(),
				face.m_albedo_coefficients_gpu.getPtr(),
				m_sh_coefficients_gpu.getPtr(),

				//device memory output
				jacobian_gpu.getPtr(), residuals_gpu.getPtr()
			);

			unmapRenderTargets(face);

			//Apply step and update poses GPU
			solveUpdatePCG(m_cublas, nUnknowns, n_current_residuals, nResiduals, jacobian_gpu, residuals_gpu, result_gpu, 1.0f, -1.0f);
			util::copy(result, result_gpu, nUnknowns);

			updateParameters(result, projection, frame.cols / static_cast<float>(frame.rows), face, nShapeCoeffs, nExpressionCoeffs, nAlbedoCoeffs);

			std::vector<float> residuals_loss_test(n_current_residuals);
			util::copy(residuals_loss_test, residuals_gpu, n_current_residuals);
			Eigen::Map<Eigen::VectorXf> residuals_loss_test_eigen(residuals_loss_test.data(), n_current_residuals);
			std::cout << "Unknowns: " << nUnknowns << ", Residuals: " << nResiduals << std::endl;
			std::cout << "Iteration: " << iteration << " , Loss: " << glm::sqrt(residuals_loss_test_eigen.dot(residuals_loss_test_eigen)) << std::endl;
		}
	}
}

void GaussNewtonSolver::solveUpdatePCG(const cublasHandle_t& cublas, const int nUnknowns, const int nCurrentResiduals, const int nResiduals, util::DeviceArray<float>& jacobian,
	util::DeviceArray<float>& residuals, util::DeviceArray<float>& x, const float alphaLHS, const float alphaRHS)
{
	const float alpha = 1, beta = 0;
	x.memset(0);

	auto r = util::DeviceArray<float>(nUnknowns);	//current residual
	auto p = util::DeviceArray<float>(nUnknowns);	//gradient
	auto M = util::DeviceArray<float>(nUnknowns);	//preconditioner
	M.memset(0);
	auto z = util::DeviceArray<float>(nUnknowns);	//preconditioned residual
	auto Jp = util::DeviceArray<float>(nCurrentResiduals);
	auto JTJp = util::DeviceArray<float>(nUnknowns);

	//M=inv(diag(JTJ))
	computeJacobiPreconditioner(nUnknowns, nCurrentResiduals, nResiduals, jacobian.getPtr(), M.getPtr());

	//r = -JTf;
	cublasSgemv(cublas, CUBLAS_OP_T, nCurrentResiduals, nUnknowns, &alphaRHS, jacobian.getPtr(), nCurrentResiduals, residuals.getPtr(), 1, &beta, r.getPtr(), 1);

	//z = Mr
	elementwiseMultiplication(nUnknowns, M.getPtr(), r.getPtr(), z.getPtr());

	//p=z;
	cublasScopy(cublas, nUnknowns, z.getPtr(), 1, p.getPtr(), 1);

	float zTr_old = 0, zTr = 0;
	float pTJTJp;
	//zTr
	cublasSdot(cublas, nUnknowns, z.getPtr(), 1, r.getPtr(), 1, &zTr_old);
	int i = 0;
	for (; i < std::min(nUnknowns, m_params.num_pcg_iterations); ++i)
	{
		//apply JTJ
		cublasSgemv(cublas, CUBLAS_OP_N, nCurrentResiduals, nUnknowns, &alphaLHS, jacobian.getPtr(), nCurrentResiduals, p.getPtr(), 1, &beta, Jp.getPtr(), 1);
		cublasSgemv(cublas, CUBLAS_OP_T, nCurrentResiduals, nUnknowns, &alpha, jacobian.getPtr(), nCurrentResiduals, Jp.getPtr(), 1, &beta, JTJp.getPtr(), 1);

		cublasSdot(cublas, nUnknowns, p.getPtr(), 1, JTJp.getPtr(), 1, &pTJTJp);

		float ak = zTr_old / std::max(pTJTJp, m_params.kNearZero);
		//x = ak*p + x
		cublasSaxpy(cublas, nUnknowns, &ak, p.getPtr(), 1, x.getPtr(), 1);

		//r = r - ak* JTJp
		ak *= -1;
		cublasSaxpy(cublas, nUnknowns, &ak, JTJp.getPtr(), 1, r.getPtr(), 1);

		//z=Mr
		elementwiseMultiplication(nUnknowns, M.getPtr(), r.getPtr(), z.getPtr());

		//zTr
		cublasSdot(cublas, nUnknowns, z.getPtr(), 1, r.getPtr(), 1, &zTr);

		if (zTr < m_params.kTolerance)
		{
			break;
		}

		float bk = zTr / std::max(zTr_old, m_params.kNearZero);

		//p = z + bk*p
		cublasSscal(cublas, nUnknowns, &bk, p.getPtr(), 1);
		cublasSaxpy(cublas, nUnknowns, &alpha, z.getPtr(), 1, p.getPtr(), 1);

		zTr_old = zTr;
	}
	//	std::cout << "PCG iters: " << i << std::endl;
}

void GaussNewtonSolver::solveUpdateCG(const cublasHandle_t& cublas, const int nUnknowns, const int nResiduals, util::DeviceArray<float>& jacobian,
	util::DeviceArray<float>& residuals, util::DeviceArray<float>& x, const float alphaLHS, const float alphaRHS)
{
	const float alpha = 1, beta = 0;
	x.memset(0);

	auto r = util::DeviceArray<float>(nUnknowns);	//current residual
	auto p = util::DeviceArray<float>(nUnknowns);	//gradient
	auto Jp = util::DeviceArray<float>(nResiduals);
	auto JTJp = util::DeviceArray<float>(nUnknowns);

	//r = -JTf;
	cublasSgemv(cublas, CUBLAS_OP_T, nResiduals, nUnknowns, &alphaRHS, jacobian.getPtr(), nResiduals, residuals.getPtr(), 1, &beta, r.getPtr(), 1);

	//p=r;
	cublasScopy(cublas, nUnknowns, r.getPtr(), 1, p.getPtr(), 1);

	float rTr_old = 0, rTr;
	float pTJTJp;
	//rTr
	cublasSdot(cublas, nUnknowns, r.getPtr(), 1, r.getPtr(), 1, &rTr);
	int i = 0;
	auto num_of_iterations = std::min(nUnknowns, m_params.num_pcg_iterations);
	for (; i < num_of_iterations; ++i)
	{
		//apply JTJ
		cublasSgemv(cublas, CUBLAS_OP_N, nResiduals, nUnknowns, &alphaLHS, jacobian.getPtr(), nResiduals, p.getPtr(), 1, &beta, Jp.getPtr(), 1);
		cublasSgemv(cublas, CUBLAS_OP_T, nResiduals, nUnknowns, &alpha, jacobian.getPtr(), nResiduals, Jp.getPtr(), 1, &beta, JTJp.getPtr(), 1);

		rTr_old = rTr;

		cublasSdot(cublas, nUnknowns, p.getPtr(), 1, JTJp.getPtr(), 1, &pTJTJp);

		float ak = rTr / std::max(pTJTJp, m_params.kNearZero);
		//x = ak*p + x
		cublasSaxpy(cublas, nUnknowns, &ak, p.getPtr(), 1, x.getPtr(), 1);

		//r = r - ak* JTJp
		ak *= -1;
		cublasSaxpy(cublas, nUnknowns, &ak, JTJp.getPtr(), 1, r.getPtr(), 1);

		//rTr
		cublasSdot(cublas, nUnknowns, r.getPtr(), 1, r.getPtr(), 1, &rTr);

		if (rTr < m_params.kTolerance)
		{
			break;
		}

		float bk = rTr / std::max(rTr_old, m_params.kNearZero);

		//p = r + bk*p
		cublasSscal(cublas, nUnknowns, &bk, p.getPtr(), 1);
		cublasSaxpy(cublas, nUnknowns, &alpha, r.getPtr(), 1, p.getPtr(), 1);
	}
}

void GaussNewtonSolver::updateParameters(const std::vector<float>& result, glm::mat4& projection, float aspect_ratio, Face& face,
	const int nShapeCoeffs, const int nExpressionCoeffs, const int nAlbedoCoeffs)
{
	projection[0][0] += result[0];
	projection[1][1] = projection[0][0] * aspect_ratio;

	face.m_rotation_coefficients.x += result[1];
	face.m_rotation_coefficients.y += result[2];
	face.m_rotation_coefficients.z += result[3];

	face.m_translation_coefficients.x += result[4];
	face.m_translation_coefficients.y += result[5];
	face.m_translation_coefficients.z += result[6];

#pragma omp parallel num_threads(4)
	{
#pragma omp single
		{
			for (int i = 0; i < nShapeCoeffs; ++i)
			{
				face.m_shape_coefficients[i] += result[7 + i];
			}
		}

#pragma omp single
		{
			for (int i = 0; i < nExpressionCoeffs; ++i)
			{
				auto c = face.m_expression_coefficients[i] + result[7 + nShapeCoeffs + i];
				face.m_expression_coefficients[i] = glm::clamp(c, -0.5f, 0.5f);
			}
		}

#pragma omp single
		{
			for (int i = 0; i < nAlbedoCoeffs; ++i)
			{
				face.m_albedo_coefficients[i] += result[7 + nShapeCoeffs + nExpressionCoeffs + i];
			}
		}

#pragma omp single
		{
			for (int i = 0; i < 9; ++i)
			{
				face.m_sh_coefficients[i] += result[7 + nShapeCoeffs + nExpressionCoeffs + nAlbedoCoeffs + i];
			}
		}
	}
}

void GaussNewtonSolver::mapRenderTargets(Face& face)
{
	if (face.m_graphics_settings.mapped_to_cuda)
	{
		std::cout << "Warning: mapRenderTargets is called while rts is already mapped!" << std::endl;
		return;
	}

	cudaGraphicsResource* resources[] = { face.m_graphics_settings.rt_rgb_cuda_resource,
		face.m_graphics_settings.rt_barycentrics_cuda_resource,
		face.m_graphics_settings.rt_vertex_ids_cuda_resource };
	CHECK_CUDA_ERROR(cudaGraphicsMapResources(3, resources, 0));

	cudaArray* array_rgb{ nullptr };
	cudaArray* array_barycentrics{ nullptr };
	cudaArray* array_vertex_ids{ nullptr };

	CHECK_CUDA_ERROR(cudaGraphicsSubResourceGetMappedArray(&array_rgb, resources[0], 0, 0));
	CHECK_CUDA_ERROR(cudaGraphicsSubResourceGetMappedArray(&array_barycentrics, resources[1], 0, 0));
	CHECK_CUDA_ERROR(cudaGraphicsSubResourceGetMappedArray(&array_vertex_ids, resources[2], 0, 0));

	//RGB texture
	cudaResourceDesc res_desc;
	memset(&res_desc, 0, sizeof(res_desc));
	res_desc.resType = cudaResourceTypeArray;
	res_desc.res.array.array = array_rgb;

	cudaTextureDesc tex_desc;
	memset(&tex_desc, 0, sizeof(tex_desc));
	tex_desc.addressMode[0] = cudaTextureAddressMode(cudaAddressModeWrap);
	tex_desc.addressMode[1] = cudaTextureAddressMode(cudaAddressModeWrap);
	tex_desc.filterMode = cudaTextureFilterMode(cudaFilterModeLinear);
	tex_desc.readMode = cudaReadModeNormalizedFloat;
	tex_desc.normalizedCoords = 0;
	CHECK_CUDA_ERROR(cudaCreateTextureObject(&m_texture_rgb, &res_desc, &tex_desc, nullptr));

	//Barycentrics texture
	res_desc.res.array.array = array_barycentrics;
	tex_desc.filterMode = cudaTextureFilterMode(cudaFilterModePoint);
	tex_desc.readMode = cudaReadModeElementType;
	CHECK_CUDA_ERROR(cudaCreateTextureObject(&m_texture_barycentrics, &res_desc, &tex_desc, nullptr));

	//Vertex ids texture
	res_desc.res.array.array = array_vertex_ids;
	CHECK_CUDA_ERROR(cudaCreateTextureObject(&m_texture_vertex_ids, &res_desc, &tex_desc, nullptr));

	face.m_graphics_settings.mapped_to_cuda = true;
}

void GaussNewtonSolver::unmapRenderTargets(Face& face)
{
	if (!face.m_graphics_settings.mapped_to_cuda)
	{
		std::cout << "Warning: unmapRenderTargets is called while rts is already unmapped!" << std::endl;
		return;
	}

	destroyTextures();

	cudaGraphicsResource* resources[] = { face.m_graphics_settings.rt_rgb_cuda_resource,
		face.m_graphics_settings.rt_barycentrics_cuda_resource,
		face.m_graphics_settings.rt_vertex_ids_cuda_resource };
	CHECK_CUDA_ERROR(cudaGraphicsUnmapResources(3, resources, 0));

	face.m_graphics_settings.mapped_to_cuda = false;
}

void GaussNewtonSolver::destroyTextures()
{
	if (m_texture_rgb)
	{
		CHECK_CUDA_ERROR(cudaDestroyTextureObject(m_texture_rgb));
		m_texture_rgb = 0;
	}
	if (m_texture_barycentrics)
	{
		m_texture_barycentrics = 0;
		CHECK_CUDA_ERROR(cudaDestroyTextureObject(m_texture_barycentrics));
	}
	if (m_texture_vertex_ids)
	{
		CHECK_CUDA_ERROR(cudaDestroyTextureObject(m_texture_vertex_ids));
		m_texture_vertex_ids = 0;
	}
}