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

#ifndef DART_OPTIMIZER_PROBLEM_HPP_
#define DART_OPTIMIZER_PROBLEM_HPP_

#include <cstddef>
#include <vector>

#include <Eigen/Dense>

#include "dart/optimizer/Function.hpp"

namespace dart {
namespace optimizer {

/// \brief class Problem
class Problem
{
public:
  /// \brief Constructor
  explicit Problem(std::size_t _dim = 0);

  /// \brief Destructor
  virtual ~Problem() = default;

  //--------------------------- Problem Setting --------------------------------
  /// \brief Set dimension. Note: Changing the dimension will clear out the
  /// initial guess and any seeds that have been added.
  void setDimension(std::size_t _dim);

  /// \brief Get dimension
  std::size_t getDimension() const;

  /// \brief Set initial guess for opimization parameters
  void setInitialGuess(const Eigen::VectorXd& _initGuess);

  /// \brief Set initial guess for opimization parameters
  const Eigen::VectorXd& getInitialGuess() const;

  /// \brief Add a seed for the Solver to use as a hint for the neighborhood of
  /// the solution.
  void addSeed(const Eigen::VectorXd& _seed);

  /// \brief Get a mutable reference of the seed for the specified index. If an
  /// out-of-bounds index is provided a warning will print, and a reference to
  /// the initial guess will be returned instead.
  Eigen::VectorXd& getSeed(std::size_t _index);

  /// \brief An immutable version of getSeed(std::size_t)
  const Eigen::VectorXd& getSeed(std::size_t _index) const;

  /// \brief Get a mutable reference to the full vector of seeds that this
  /// Problem currently contains
  std::vector<Eigen::VectorXd>& getSeeds();

  /// \brief An immutable version of getSeeds()
  const std::vector<Eigen::VectorXd>& getSeeds() const;

  /// \brief Clear the seeds that this Problem currently contains
  void clearAllSeeds();

  /// \brief Set lower bounds for optimization parameters
  void setLowerBounds(const Eigen::VectorXd& _lb);

  /// \brief Get lower bounds for optimization parameters
  const Eigen::VectorXd& getLowerBounds() const;

  /// \brief Set upper bounds for optimization parameters
  void setUpperBounds(const Eigen::VectorXd& _ub);

  /// \brief Get upper bounds for optimization parameters
  const Eigen::VectorXd& getUpperBounds() const;

  /// \brief Set minimum objective function
  void setObjective(FunctionPtr _obj);

  /// \brief Get objective function
  FunctionPtr getObjective() const;

  /// \brief Add equality constraint
  void addEqConstraint(FunctionPtr _eqConst);

  /// \brief Add inequality constraint. Inequality constraints must evaluate
  /// to LESS THAN or equal to zero (within some tolerance) to be satisfied.
  void addIneqConstraint(FunctionPtr _ineqConst);

  /// \brief Get number of equality constraints
  std::size_t getNumEqConstraints() const;

  /// \brief Get number of inequality constraints
  std::size_t getNumIneqConstraints() const;

  /// \brief Get equality constraint
  FunctionPtr getEqConstraint(std::size_t _idx) const;

  /// \brief Get inequality constraint
  FunctionPtr getIneqConstraint(std::size_t _idx) const;

  /// \brief Remove equality constraint
  void removeEqConstraint(FunctionPtr _eqConst);

  /// \brief Remove inequality constraint
  void removeIneqConstraint(FunctionPtr _ineqConst);

  /// \brief Remove all equality constraints
  void removeAllEqConstraints();

  /// \brief Remove all inequality constraints
  void removeAllIneqConstraints();

  //------------------------------ Result --------------------------------------
  /// \brief Set optimum value of the objective function. This function called
  ///        by Solver.
  void setOptimumValue(double _val);

  /// \brief Get optimum value of the objective function
  double getOptimumValue() const;

  /// \brief Set optimal solution. This function called by Solver.
  void setOptimalSolution(const Eigen::VectorXd& _optParam);

  /// \brief Get optimal solution
  const Eigen::VectorXd& getOptimalSolution();

protected:
  /// \brief Dimension of this problem
  std::size_t mDimension;

  /// \brief Initial guess for optimization parameters
  Eigen::VectorXd mInitialGuess;

  /// \brief Additional guess hints for the Solver.
  std::vector<Eigen::VectorXd> mSeeds;

  /// \brief Lower bounds for optimization parameters
  Eigen::VectorXd mLowerBounds;

  /// \brief Upper bounds for optimization parameters
  Eigen::VectorXd mUpperBounds;

  /// \brief Objective function
  FunctionPtr mObjective;

  /// \brief Equality constraint functions
  std::vector<FunctionPtr> mEqConstraints;

  /// \brief Inequality constraint functions
  std::vector<FunctionPtr> mIneqConstraints;

  /// \brief Optimal objective value
  double mOptimumValue;

  /// \brief Optimal solution
  Eigen::VectorXd mOptimalSolution;
};

} // namespace optimizer
} // namespace dart

#endif // #ifndef DART_OPTIMIZER_PROBLEM_HPP_


/***
第四节 可变参模板
英文：Variadic Templates ，C++11
允许模板定义中包含 0 ，到多个（任意个）模板参数。

C++17
（1）可变参函数模板
（1.1）基本含义
可变参函数模板中，传递进来的一包实参怎样展开：一般都采用递归函数的方式展开参数包。
要展开，要求，在可变参函数模板代码中，有一个参数包展开函数，以及一个同名的递归终止函数。
a)myvtfunct(10, "abc", 12.7);
b)myvtfunct( "abc", 12.7);
c)myvtfunct(12.7);
d)myvtfunct();

C++17中增加了一个语句，叫做“编译期间if语句(constexpr if)”。
if constexpr()....   //constexpr，代表“常量”的意思，或者代表“编译时求值”的意思。
myvtfunct<int,char const *,double>(int,char const *,double)
myvtfunct<char const*, double>(char const*, double)
myvtfunct<double>(double)
深入认识if constexpr
a)不满足条件的分支，也同样会被编译器编译(被编译器进行语法检查）。
b)if constexpr所指定的条件必须是常量：理解成普通if语句，只是判断条件从执行期间挪到了编译期间
总结：if constexpr的存在，完善了模板与泛型编程中的程序执行路径选择问题。

（1.2）重载
一般来说，调用普通函数和调用函数模板都合适的时候，编译器优先选择普通函数。
把握不准时，可以多做测试。
***/

#include <iostream>
//#include <boost/type_index.hpp>
using namespace std;
//#pragma warning(disable : 4996)

namespace _nmsp1
{
	void myptfunct()
	{

	}

	/**
	//可变参函数模板
	template <typename... T>       //...代表参数包
	void myvtfunct(T... args)      //T：一包类型，args：一包形参   ,T称为“可变参类型”,T中包含的是0到多个不同的类型（一包类型）
		                           //args称为一包/一堆 参数（函数模板的形参：0个到多个），每个参数的类型可以各不相同。
	{
		//收到的参数数量，sizeof...固定语法，C++11中引入，用于表示收到的模板参数个数或者类型数量。
		//sizeof...针对的只能是这种...的可变参，后面圆括号中可以是函数模板的形参args，也可以是类型模板参数T.
		cout << sizeof...(args) << endl;
		cout << sizeof...(T) << endl;

	}

	//再实现一个同名的递归终止函数(这是个真正的函数)，位置放在参数包展开函数 的前面
	void myvtfunct() //这是个普通函数，而不是函数模板
	{
		cout << "参数包展开时执行了递归终止函数myvtfunct()" << endl;
	}
	***/

	//先实现参数包展开函数
	template <typename T,typename...U>
	void myvtfunct(T firstarg, U...otherargs)       //(10, "abc", 12.7);
	{
		cout << "收到的参数值：" << firstarg << endl;
		//myvtfunct(otherargs...);                 //递归调用，注意塞进来的是一包形参，这里的...不能省略
		if constexpr (sizeof...(otherargs) > 0)    //constexpr必须有否则无法成功编译，圆括号中是常量表达式。
		{
			myvtfunct(otherargs...);               //递归调用，注意塞进来的是一包形参，这里的...不能省略
		}
		else
		{
			//不需要做什么，其实整个else都可以不要。
		}

		/*if constexpr (sizeof...(otherargs) > 100) //不管if constexpr条件是否成立，{}中的代码行都会被编译
		{
			testfunc();
		}*/
//#define _MYDEF_
//#ifdef  _MYDEF_
//		testfunc();
//#endif

		/*int i = 8;
		if constexpr (i > 8)
		{

		}*/
	}
}

namespace _nmsp2
{
	template<typename... T>
	void myfunc(T... arg)
	{
		cout << "myfunc(T... arg)执行了!" << endl;
	}

	template<typename... T>
	void myfunc(T*... arg)
	{
		cout << "myfunc(T*... arg)执行了!" << endl;
	}

	void myfunc(int arg)
	{
		cout << "myfunc(int arg)执行了!" << endl;
	}

}

int main()
{
	//_nmsp1::myvtfunct();
	//_nmsp1::myvtfunct(10,20);
	//_nmsp1::myvtfunct(10, 25.8,"abc",68);
	//_nmsp1::myvtfunct<double,double>(10, 25.8, "abc", 68,73); //指定部分类型，让编译器去推断另一部分类型，是允许的。

	//_nmsp1::myvtfunct(10, "abc", 12.7);

	_nmsp2::myfunc(NULL);
	_nmsp2::myfunc(nullptr); //nullptr是空指针
	_nmsp2::myfunc((int *)nullptr);

	return 0;
}


/**
BSD 3-Clause License

This file is part of the RootBA project.
https://github.com/NikolausDemmel/rootba

Copyright (c) 2021, Nikolaus Demmel.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#pragma once

#include <mutex>

#include <Eigen/Dense>
#include <glog/logging.h>
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_reduce.h>

#include "rootba/bal/bal_problem.hpp"
#include "rootba/qr/landmark_block.hpp"
#include "rootba/util/assert.hpp"
#include "rootba/util/cast.hpp"
#include "rootba/util/format.hpp"

namespace rootba {

template <typename Scalar_, int POSE_SIZE_>
class LinearizationQR : public LinearOperator<Scalar_> {
 public:
  using Scalar = Scalar_;
  static constexpr int POSE_SIZE = POSE_SIZE_;

  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
  using VecP = Eigen::Matrix<Scalar, POSE_SIZE, 1>;

  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;

  using LandmarkBlockPtr = std::unique_ptr<LandmarkBlock<Scalar>>;

  struct Options {
    int reduction_alg = 0;
    typename LandmarkBlock<Scalar>::Options lb_options;
  };

  LinearizationQR(BalProblem<Scalar>& bal_problem, const Options& options)
      : options_(options),
        bal_problem_(bal_problem),
        pose_mutex_(bal_problem_.cameras().size()),
        pose_damping_diagonal_(0),
        pose_damping_diagonal_sqrt_(0) {
    size_t num_landmakrs = bal_problem_.landmarks().size();

    landmark_blocks_.resize(num_landmakrs);

    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        auto& lb = landmark_blocks_[r];
        auto& landmark = bal_problem_.landmarks()[r];

        lb = LandmarkBlockFactory<Scalar, POSE_SIZE>::get_landmark_block(
            landmark.obs.size());

        lb->allocate_landmark(landmark, options.lb_options);
      }
    };

    tbb::blocked_range<size_t> range(0, num_landmakrs);
    tbb::parallel_for(range, body);

    landmark_block_idx_.reserve(num_landmakrs);

    num_rows_Q2Tr_ = 0;
    for (size_t i = 0; i < num_landmakrs; i++) {
      landmark_block_idx_.emplace_back(num_rows_Q2Tr_);
      num_rows_Q2Tr_ += landmark_blocks_[i]->num_Q2T_rows();
    }

    num_cameras_ = bal_problem_.cameras().size();
  }

  ~LinearizationQR() override = default;

  // return value `false` indicates numerical failure --> linearization at this
  // state is unusable. Numeric check is only performed for residuals that were
  // considered to be used (valid), which depends on use_valid_projections_only
  // setting.
  bool linearize_problem() {
    ROOTBA_ASSERT(bal_problem_.landmarks().size() == landmark_blocks_.size());

    size_t num_landmarks = landmark_blocks_.size();

    auto body = [&](const tbb::blocked_range<size_t>& range,
                    bool numerically_valid) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        landmark_blocks_[r]->linearize_landmark(bal_problem_.cameras());
        numerically_valid &= !landmark_blocks_[r]->is_numerical_failure();
      }
      return numerically_valid;
    };

    tbb::blocked_range<size_t> range(0, num_landmarks);
    const bool numerically_valid =
        tbb::parallel_reduce(range, true, body, std::logical_and<>());

    return numerically_valid;
  }

  void set_pose_damping(const Scalar lambda) {
    ROOTBA_ASSERT(lambda >= 0);

    pose_damping_diagonal_ = lambda;
    pose_damping_diagonal_sqrt_ = std::sqrt(lambda);
  }

  bool has_pose_damping() const { return pose_damping_diagonal_ > 0; }

  size_t num_rows_reduced() const {
    return has_pose_damping() ? num_rows_Q2Tr_ + num_cameras_ * POSE_SIZE
                              : num_rows_Q2Tr_;
  }

  size_t num_cols_reduced() const { return num_cameras_ * POSE_SIZE; }

  void perform_qr() {
    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        landmark_blocks_[r]->perform_qr();
      }
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);
  }

  Scalar back_substitute(const VecX& pose_inc) {
    ROOTBA_ASSERT(pose_inc.size() == signed_cast(num_cameras_ * POSE_SIZE));

    auto body = [&](const tbb::blocked_range<size_t>& range, Scalar l_diff) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        landmark_blocks_[r]->back_substitute(pose_inc, l_diff);
      }
      return l_diff;
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    Scalar l_diff =
        tbb::parallel_reduce(range, Scalar(0), body, std::plus<Scalar>());
    return l_diff;
  }

  VecX get_Q2Tr() const {
    VecX res(num_rows_reduced());

    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        const auto& lb = landmark_blocks_[r];
        res.segment(landmark_block_idx_[r], lb.numQ2rows()) = lb.getQ2r();
      }
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);

    if (has_pose_damping()) {
      res.tail(num_cameras_ * POSE_SIZE).setZero();
    }

    return res;
  }

  Eigen::SparseMatrix<Scalar, Eigen::RowMajor> get_Q2TJp() const {
    // Since we know exactly the sparsity, we can do better than triplets:
    // https://stackoverflow.com/a/18160211/1813258
    // https://eigen.tuxfamily.org/dox/group__SparseQuickRefPage.html
    // But since it's for debugging, it probably doesn't matter...

    // prepare triplet vector
    size_t num_triplets = 0;
    for (const auto& lb : landmark_blocks_) {
      num_triplets += lb->num_reduced_cams() * POSE_SIZE * lb->num_Q2T_rows();
    }
    if (has_pose_damping()) {
      num_triplets += num_cameras_ * POSE_SIZE;
    }
    std::vector<Eigen::Triplet<Scalar>> triplets;
    triplets.reserve(num_triplets);

    // generate all triplets
    for (size_t i = 0; i < landmark_blocks_.size(); ++i) {
      const auto& lb = landmark_blocks_[i];
      lb->add_triplets_Q2TJp(landmark_block_idx_[i], triplets);
    }

    // add dapening entries
    if (has_pose_damping()) {
      for (size_t i = 0; i < num_cameras_ * POSE_SIZE; ++i) {
        triplets.emplace_back(num_rows_Q2Tr_ + i, i,
                              pose_damping_diagonal_sqrt_);
      }
    }

    // build sparse matrix
    Eigen::SparseMatrix<Scalar, Eigen::RowMajor> res(num_rows_reduced(),
                                                     POSE_SIZE * num_cameras_);
    if (!triplets.empty()) {
      res.setFromTriplets(triplets.begin(), triplets.end());
    }

    return res;
  }

  VecX get_Q2TJp_postmult_x(const VecX& x_pose) const {
    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));

    VecX res(num_rows_reduced());

    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        res.segment(landmark_block_idx_[r],
                    landmark_blocks_[r]->num_Q2T_rows()) =
            landmark_blocks_[r]->get_Q2TJp_postmult_x(x_pose);
      }
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);

    if (has_pose_damping()) {
      res.tail(POSE_SIZE * num_cameras_) =
          x_pose.array() * pose_damping_diagonal_sqrt_;
    }

    return res;
  }

  VecX get_Q2TJp_premult_x(const VecX& x_r) const {
    ROOTBA_ASSERT(x_r.size() == signed_cast(num_rows_reduced()));

    auto body = [&](const tbb::blocked_range<size_t>& range, VecX res) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        const auto& lb = landmark_blocks_[r];

        lb->add_Q2TJp_premult_x(
            res, x_r.segment(landmark_block_idx_[r], lb->num_Q2T_rows()));
      }
      return res;
    };

    auto join = [](const auto& x, const auto& y) { return x + y; };

    VecX init;
    init.setZero(POSE_SIZE * num_cameras_);

    // go over all host frames
    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    VecX res = tbb::parallel_reduce(range, init, body, join);

    if (has_pose_damping()) {
      res += (x_r.tail(POSE_SIZE * num_cameras_).array() *
              pose_damping_diagonal_sqrt_)
                 .matrix();
    }

    return res;
  }

  inline VecX get_Q2TJp_T_Q2TJp_mult_x(const VecX& x_pose) const {
    switch (options_.reduction_alg) {
      case 0:
        return get_Q2TJp_T_Q2TJp_mult_x_v0(x_pose);
        //      case 1:
        //        return getQ2JpTQ2Jp_mult_x_v1(x_pose);
        //      case 2:
        //        return getQ2JpTQ2Jp_mult_x_v2(x_pose);
      case 3:
        return get_Q2TJp_T_Q2TJp_mult_x_v3(x_pose);

      default:
        LOG(FATAL) << "options_.reduction_alg " << options_.reduction_alg
                   << " is not supported.";
    }
  }

  VecX get_Q2TJp_T_Q2TJp_mult_x_v0(const VecX& x_pose) const {
    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));

    struct Reductor {
      Reductor(const VecX& x_pose,
               const std::vector<LandmarkBlockPtr>& landmark_blocks)
          : x_pose(x_pose), landmark_blocks(landmark_blocks) {
        res.setZero(x_pose.rows());
      }

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          const auto& lb = landmark_blocks[r];
          lb->add_Q2TJp_T_Q2TJp_mult_x(res, x_pose);
        }
      }

      Reductor(Reductor& a, tbb::split /*unused*/)
          : x_pose(a.x_pose), landmark_blocks(a.landmark_blocks) {
        res.setZero(x_pose.rows());
      }

      inline void join(const Reductor& b) { res += b.res; }

      const VecX& x_pose;
      const std::vector<LandmarkBlockPtr>& landmark_blocks;
      VecX res;
    };

    Reductor r(x_pose, landmark_blocks_);

    // go over all host frames
    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    if (has_pose_damping()) {
      r.res += (x_pose.array() * pose_damping_diagonal_).matrix();
    }

    return r.res;
  }

  // Version with unordered_map and mutex guarded writes
  VecX get_Q2TJp_T_Q2TJp_mult_x_v1(const VecX& x_pose) const {
    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));

    VecX res;
    res.setZero(x_pose.rows());

    auto body = [&](const tbb::blocked_range<size_t>& range) {
      std::unordered_map<int, VecP> partial_sum_map;
      partial_sum_map.reserve(num_cameras_ / 8);

      for (size_t r = range.begin(); r != range.end(); ++r) {
        const auto& lb = landmark_blocks_[r];
        // lb->mapQ2JpTQ2Jp_mult_x(partial_sum_map, x_pose);
      }

      for (const auto& [k, v] : partial_sum_map) {
        std::scoped_lock<std::mutex> lock(pose_mutex_[k]);
        res.template segment<POSE_SIZE>(k * POSE_SIZE) += v;
      }

      return res;
    };

    // go over all host frames
    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);

    if (has_pose_damping()) {
      res += (x_pose.array() * pose_damping_diagonal_).matrix();
    }

    return res;
  }

  // Verstion with map and mutex guarded writes
  VecX get_Q2TJp_T_Q2TJp_mult_x_v2(const VecX& x_pose) const {
    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));

    VecX res;
    res.setZero(x_pose.rows());

    auto body = [&](const tbb::blocked_range<size_t>& range) {
      std::map<int, VecP> partial_sum_map;

      for (size_t r = range.begin(); r != range.end(); ++r) {
        const auto& lb = landmark_blocks_[r];
        // lb->mapQ2JpTQ2Jp_mult_x(partial_sum_map, x_pose);
      }

      for (const auto& [k, v] : partial_sum_map) {
        std::scoped_lock<std::mutex> lock(pose_mutex_[k]);
        res.template segment<POSE_SIZE>(k * POSE_SIZE) += v;
      }

      return res;
    };

    // go over all host frames
    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);

    if (has_pose_damping()) {
      res += (x_pose.array() * pose_damping_diagonal_).matrix();
    }

    return res;
  }

  // Verstion with direct mutex guarded writes
  VecX get_Q2TJp_T_Q2TJp_mult_x_v3(const VecX& x_pose) const {
    ROOTBA_ASSERT(x_pose.size() == signed_cast(num_cameras_ * POSE_SIZE));

    VecX res;
    res.setZero(x_pose.rows());

    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        const auto& lb = landmark_blocks_[r];
        lb->add_Q2TJp_T_Q2TJp_mult_x(res, x_pose, &pose_mutex_);
      }

      return res;
    };

    // go over all host frames
    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);

    if (has_pose_damping()) {
      res += (x_pose.array() * pose_damping_diagonal_).matrix();
    }

    return res;
  }

  VecX get_Q2TJp_T_Q2Tr() const {
    auto body = [&](const tbb::blocked_range<size_t>& range, VecX res) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        const auto& lb = landmark_blocks_[r];
        lb->add_Q2TJp_T_Q2Tr(res);
      }
      return res;
    };

    auto join = [](const auto& x, const auto& y) { return x + y; };

    VecX init;
    init.setZero(POSE_SIZE * num_cameras_);

    // go over all host frames
    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    VecX res = tbb::parallel_reduce(range, init, body, join);

    // Note: No need to consider pose damping, since the residual part is 0

    return res;
  }

  VecX get_Q2TJp_diag2() const {
    struct Reductor {
      Reductor(size_t num_rows,
               const std::vector<LandmarkBlockPtr>& landmark_blocks)
          : num_rows(num_rows), landmark_blocks(landmark_blocks) {
        res.setZero(num_rows);
      }

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          const auto& lb = landmark_blocks[r];
          lb->add_Q2TJp_diag2(res);
        }
      }

      Reductor(Reductor& a, tbb::split /*unused*/)
          : num_rows(a.num_rows), landmark_blocks(a.landmark_blocks) {
        res.setZero(num_rows);
      }

      inline void join(const Reductor& b) { res += b.res; }

      size_t num_rows;
      const std::vector<LandmarkBlockPtr>& landmark_blocks;
      VecX res;
    };

    Reductor r(num_cameras_ * POSE_SIZE, landmark_blocks_);

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    if (has_pose_damping()) {
      r.res.array() += pose_damping_diagonal_;
    }

    return r.res;
  }

  VecX get_Jp_diag2() const {
    struct Reductor {
      Reductor(size_t num_rows,
               const std::vector<LandmarkBlockPtr>& landmark_blocks)
          : num_rows(num_rows), landmark_blocks(landmark_blocks) {
        res.setZero(num_rows);
      }

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          const auto& lb = landmark_blocks[r];
          lb->add_Jp_diag2(res);
        }
      }

      Reductor(Reductor& a, tbb::split /*unused*/)
          : num_rows(a.num_rows), landmark_blocks(a.landmark_blocks) {
        res.setZero(num_rows);
      }

      inline void join(const Reductor& b) { res += b.res; }

      size_t num_rows;
      const std::vector<LandmarkBlockPtr>& landmark_blocks;
      VecX res;
    };

    Reductor r(num_cameras_ * POSE_SIZE, landmark_blocks_);

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    // TODO: double check including vs not including pose damping here in usage
    // and make it clear in API; see also getJpTJp_blockdiag

    // Note: ignore damping here

    return r.res;
  }

  IndexedBlocks<Scalar> get_Q2TJp_T_Q2TJp_blockdiag() const {
    struct Reductor {
      Reductor(const std::vector<LandmarkBlockPtr>& landmark_blocks)
          : landmark_blocks(landmark_blocks) {}

      Reductor(Reductor& a, tbb::split /*unused*/)
          : landmark_blocks(a.landmark_blocks) {}

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          const auto& lb = landmark_blocks[r];
          lb->add_Q2TJp_T_Q2TJp_blockdiag(accum);
        }
      }

      inline void join(Reductor& b) { accum.join(b.accum); }

      BlockDiagonalAccumulator<Scalar> accum;
      const std::vector<LandmarkBlockPtr>& landmark_blocks;
    };

    Reductor r(landmark_blocks_);

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    if (has_pose_damping()) {
      r.accum.add_diag(
          num_cameras_, POSE_SIZE,
          VecX::Constant(num_cameras_ * POSE_SIZE, pose_damping_diagonal_));
    }

    return r.accum.block_diagonal;
  }

  IndexedBlocks<Scalar> get_Jp_T_Jp_blockdiag() const {
    struct Reductor {
      Reductor(const std::vector<LandmarkBlockPtr>& landmark_blocks)
          : landmark_blocks(landmark_blocks) {}

      Reductor(Reductor& a, tbb::split /*unused*/)
          : landmark_blocks(a.landmark_blocks) {}

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          const auto& lb = landmark_blocks[r];
          lb->add_Jp_T_Jp_blockdiag(accum);
        }
      }

      inline void join(Reductor& b) { accum.join(b.accum); }

      BlockDiagonalAccumulator<Scalar> accum;
      const std::vector<LandmarkBlockPtr>& landmark_blocks;
    };

    Reductor r(landmark_blocks_);

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    if (has_pose_damping()) {
      r.accum.add_diag(
          num_cameras_, POSE_SIZE,
          VecX::Constant(num_cameras_ * POSE_SIZE, pose_damping_diagonal_));
    }

    // TODO: double check including vs not including pose damping here in usage
    // and make it clear in API; see also getJp_diag2

    return r.accum.block_diagonal;
  }

  void scale_Jl_cols() {
    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        landmark_blocks_[r]->scale_Jl_cols();
      }
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);
  }

  void scale_Jp_cols(const VecX& jacobian_scaling) {
    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        landmark_blocks_[r]->scale_Jp_cols(jacobian_scaling);
      }
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);
  }

  void set_landmark_damping(Scalar lambda) {
    auto body = [&](const tbb::blocked_range<size_t>& range) {
      for (size_t r = range.begin(); r != range.end(); ++r) {
        landmark_blocks_[r]->set_landmark_damping(lambda);
      }
    };

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_for(range, body);
  }

  // Operations grouped into stages. Stage1 includes linearization, optional
  // JACOBI preconditioner, QR and column norms computations for LM damping.
  VecX get_stage1(IndexedBlocks<Scalar>* precond_block_diagonal) {
    struct Reductor {
      Reductor(size_t num_rows, bool precond_block_diagonal,
               std::vector<LandmarkBlockPtr>& landmark_blocks,
               const BalProblem<Scalar>& bal_problem)
          : num_rows(num_rows),
            landmark_blocks(landmark_blocks),
            bal_problem(bal_problem) {
        if (precond_block_diagonal) {
          precond_block_diagonal_accum.reset(
              new BlockDiagonalAccumulator<Scalar>());
        }
        res.setZero(num_rows);
      }

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          auto& lb = landmark_blocks[r];
          lb->linearize_landmark(bal_problem.cameras());
          if (!lb->is_numerical_failure()) {
            if (precond_block_diagonal_accum) {
              lb->add_Jp_T_Jp_blockdiag(*precond_block_diagonal_accum);
            }
            lb->add_Jp_diag2(res);
            lb->scale_Jl_cols();
            lb->perform_qr();
          } else {
            numerically_valid = false;
          }
        }
      }

      Reductor(Reductor& a, tbb::split /*unused*/)
          : num_rows(a.num_rows),
            landmark_blocks(a.landmark_blocks),
            bal_problem(a.bal_problem) {
        if (a.precond_block_diagonal_accum) {
          precond_block_diagonal_accum.reset(
              new BlockDiagonalAccumulator<Scalar>());
        }
        res.setZero(num_rows);
      }

      inline void join(Reductor& b) {
        if (precond_block_diagonal_accum) {
          precond_block_diagonal_accum->join(*b.precond_block_diagonal_accum);
        }
        res += b.res;
        numerically_valid &= b.numerically_valid;
      }

      size_t num_rows;
      std::vector<LandmarkBlockPtr>& landmark_blocks;
      const BalProblem<Scalar>& bal_problem;
      std::unique_ptr<BlockDiagonalAccumulator<Scalar>>
          precond_block_diagonal_accum;
      VecX res;
      bool numerically_valid = true;
    };

    const bool compute_precond_block_diagonal = precond_block_diagonal;

    Reductor r(num_cameras_ * POSE_SIZE, compute_precond_block_diagonal,
               landmark_blocks_, bal_problem_);

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    if (r.numerically_valid) {
      if (compute_precond_block_diagonal) {
        *precond_block_diagonal =
            std::move(r.precond_block_diagonal_accum->block_diagonal);
      }

      return r.res;
    } else {
      return {};  // for now, return empty vector to indicate numerical failure
    }
  }

  // Operations grouped into stages. Stage2 includes Landmark damping, bref and
  // precondition computations.
  void get_stage2(Scalar lambda, const VecX* jacobian_scaling,
                  IndexedBlocks<Scalar>* precond_block_diagonal, VecX& bref) {
    struct Reductor {
      Reductor(size_t num_rows, Scalar lambda, const VecX* jacobian_scaling,
               bool precond_block_diagonal,
               std::vector<LandmarkBlockPtr>& landmark_blocks)
          : num_rows(num_rows),
            lambda(lambda),
            jacobian_scaling(jacobian_scaling),
            landmark_blocks(landmark_blocks) {
        if (precond_block_diagonal) {
          precond_block_diagonal_accum.reset(
              new BlockDiagonalAccumulator<Scalar>());
        }
        bref.setZero(num_rows);
      }

      void operator()(const tbb::blocked_range<size_t>& range) {
        for (size_t r = range.begin(); r != range.end(); ++r) {
          auto& lb = landmark_blocks[r];

          lb->stage2(lambda, jacobian_scaling,
                     precond_block_diagonal_accum.get(), bref);

          //          // 1. scale jacobian
          //          if (jacobian_scaling_) {
          //            lb->scaleJp_cols(*jacobian_scaling_);
          //          }

          //          // 2. dampen landmarks
          //          lb->setLandmarkDamping(lambda_);

          //          // 3. compute block diagonal preconditioner
          //          (SCHUR_JACOBI) if (precond_block_diagonal_accum_) {
          //            lb->addQ2JpTQ2Jp_blockdiag(*precond_block_diagonal_accum_);
          //          }

          //          // 4. compute rhs of reduced camera normal equations
          //          lb->addQ2JpTQ2r(bref_);
        }
      }

      Reductor(Reductor& a, tbb::split /*unused*/)
          : num_rows(a.num_rows),
            lambda(a.lambda),
            jacobian_scaling(a.jacobian_scaling),
            landmark_blocks(a.landmark_blocks) {
        if (a.precond_block_diagonal_accum) {
          precond_block_diagonal_accum.reset(
              new BlockDiagonalAccumulator<Scalar>());
        }
        bref.setZero(num_rows);
      }

      inline void join(Reductor& b) {
        if (precond_block_diagonal_accum) {
          precond_block_diagonal_accum->join(*b.precond_block_diagonal_accum);
        }
        bref += b.bref;
      }

      size_t num_rows;
      Scalar lambda;
      const VecX* jacobian_scaling;
      std::vector<LandmarkBlockPtr>& landmark_blocks;

      std::unique_ptr<BlockDiagonalAccumulator<Scalar>>
          precond_block_diagonal_accum;
      VecX bref;
    };

    const bool compute_precond_block_diagonal = precond_block_diagonal;

    Reductor r(num_cameras_ * POSE_SIZE, lambda, jacobian_scaling,
               compute_precond_block_diagonal, landmark_blocks_);

    tbb::blocked_range<size_t> range(0, landmark_block_idx_.size());
    tbb::parallel_reduce(range, r);

    // add pose damping to preconditioners
    if (has_pose_damping()) {
      if (compute_precond_block_diagonal) {
        r.precond_block_diagonal_accum->add_diag(
            num_cameras_, POSE_SIZE,
            VecX::Constant(num_cameras_ * POSE_SIZE, pose_damping_diagonal_));
      }
    }

    // TODO: try these kinds of optimizations with std::move here and elsewhere:
    // precond_block_diagonal =
    //   std::move(r.precond_block_diagonal_accum->block_diagonal);
    // bref = std::move(r.bref);

    // move compute results to output variables
    if (compute_precond_block_diagonal) {
      *precond_block_diagonal =
          std::move(r.precond_block_diagonal_accum->block_diagonal);
    }
    bref = std::move(r.bref);
  }

  void print_block(const std::string& filename, size_t block_idx) {
    landmark_blocks_[block_idx].printStorage(filename);
  }

  size_t num_cols() const override { return this->num_cols_reduced(); }

  VecX right_multiply(const VecX& x) const override {
    return this->get_Q2TJp_T_Q2TJp_mult_x(x);
  }

 protected:
  Options options_;

  std::vector<LandmarkBlockPtr> landmark_blocks_;
  std::vector<size_t> landmark_block_idx_;
  BalProblem<Scalar>& bal_problem_;

  mutable std::vector<std::mutex> pose_mutex_;

  Scalar pose_damping_diagonal_;
  Scalar pose_damping_diagonal_sqrt_;

  size_t num_cameras_;
  size_t num_rows_Q2Tr_;
};

}  // namespace rootba


#pragma once

#include <string>
#include <sstream>
#include <math/math.hpp>
#include <boost/lexical_cast.hpp>

namespace boost
{

    namespace detail
    {

        template<class Type>
        struct base_conversion
        {
            static std::string save_to_string(const Type &arg)
            {
                std::ostringstream str;

                for
                (
                    typename Type::const_iterator begin = arg.begin(), it = begin, end = arg.end();
                    it != end;
                    ++it
                )
                {
                    if (it != begin) str << ' ';
                    str << *it;
                }

                return str.str();
            }
        };

        template<class Type, typename InternalType = typename Type::value_type>
        struct conversion
        {
            static Type load_from_cstr(char *start)
            {
                throw_exception(bad_lexical_cast(typeid(std::string), typeid(Type)));
            }
        };

        template<class Type>
        struct conversion<Type, float> : public base_conversion<Type>
        {
            static Type load_from_cstr(char *start)
            {
                Type r;

                char *stop  = start;

                for
                (
                    typename Type::iterator it = r.begin(), end = r.end();
                    it != end;
                    ++it
                )
                {
                    *it = typename Type::value_type(strtod(start, &stop));
                    if (start != stop)
                        start = stop;
                    else
                        throw_exception(bad_lexical_cast(typeid(std::string), typeid(Type)));
                }

                return r;
            }
        };

        template<class Type>
        struct conversion<Type, double> : public base_conversion<Type>
        {
            static Type load_from_cstr(char *str)
            {
                Type r;

                char *start = str;
                char *stop  = start;

                for
                (
                    typename Type::iterator it = r.begin(), end = r.end();
                    it != end;
                    ++it
                )
                {
                    *it = typename Type::value_type(strtod(start, &stop));
                    if (start != stop)
                        start = stop;
                    else
                        throw_exception(bad_lexical_cast(typeid(std::string), typeid(Type)));
                }

                return r;
            }
        };

        template<class Type>
        struct conversion<Type, int> : public base_conversion<Type>
        {
            static Type load_from_cstr(char *str)
            {
                Type r;

                char *start = str;
                char *stop  = start;

                for
                (
                    typename Type::iterator it = r.begin(), end = r.end();
                    it != end;
                    ++it
                )
                {
                    *it = typename Type::value_type(strtol(start, &stop, 10));
                    if (start != stop)
                        start = stop;
                    else
                        throw bad_lexical_cast(typeid(std::string), typeid(Type));
                }

                return r;
            }
        };

        template<class Type>
        struct conversion<Type, long> : public base_conversion<Type>
        {
            static Type load_from_cstr(char *str)
            {
                Type r;

                char *start = str;
                char *stop  = start;

                for
                (
                    typename Type::iterator it = r.begin(), end = r.end();
                    it != end;
                    ++it
                )
                {
                    *it = typename Type::value_type(strtol(start, &stop, 10));
                    if (start != stop)
                        start = stop;
                    else
                        throw_exception(bad_lexical_cast(typeid(std::string), typeid(Type)));
                }

                return r;
            }
        };

    }

    template<>
    inline nMath::Vector2f lexical_cast<nMath::Vector2f, std::string>(const std::string &str)
    { return detail::conversion<nMath::Vector2f>::load_from_cstr(const_cast<char*>(str.c_str())); }

    template<>
    inline std::string lexical_cast<std::string, nMath::Vector2f>(const nMath::Vector2f &v)
    { return detail::conversion<nMath::Vector2f>::save_to_string(v); }

    template<>
    inline nMath::Vector3f lexical_cast<nMath::Vector3f, std::string>(const std::string &str)
    { return detail::conversion<nMath::Vector3f>::load_from_cstr(const_cast<char*>(str.c_str())); }

    template<>
    inline std::string lexical_cast<std::string, nMath::Vector3f>(const nMath::Vector3f &v)
    { return detail::conversion<nMath::Vector3f>::save_to_string(v); }

    template<>
    inline nMath::Vector4f lexical_cast<nMath::Vector4f, std::string>(const std::string &str)
    { return detail::conversion<nMath::Vector4f>::load_from_cstr(const_cast<char*>(str.c_str())); }

    template<>
    inline std::string lexical_cast<std::string, nMath::Vector4f>(const nMath::Vector4f &v)
    { return detail::conversion<nMath::Vector4f>::save_to_string(v); }

    template<>
    inline nMath::Quatf lexical_cast<nMath::Quatf, std::string>(const std::string &str)
    { return detail::conversion<nMath::Quatf>::load_from_cstr(const_cast<char*>(str.c_str())); }

    template<>
    inline std::string lexical_cast<std::string, nMath::Quatf>(const nMath::Quatf &v)
    { return detail::conversion<nMath::Quatf>::save_to_string(v); }

}


#include <iostream>
//#include <map>
//#include <vector>
//#include <list>
//#include <functional>
//
//#include <boost/type_index.hpp>

using namespace std;
//
//#pragma warning(disable : 4996)

namespace _nmsp1
{
	//泛化：大众化，常规
	template <typename T,typename U>
	struct TC
	{
		TC()
		{
			cout << "TC泛化版本构造函数" << endl;
		}
		void functest1()
		{
			cout << "functest1泛化版本" << endl;
		}

		static int m_stc;  //声明一个静态成员变量
	};

	template <>
	void TC<double, int>::functest1() //functest1成员函数的全特化版本
	{
		cout << "普通成员函数TC<double,int>::functest1的全特化" << endl;
	}

	template <typename T, typename U>
	int TC<T,U>::m_stc = 50; //定义静态成员变量

	template <>
	int TC<double, int>::m_stc = 100;

	//-------------------
	/*template <>
	struct TC<double, int>
	{

	};*/

	//-------------------
	template <> //全特化：所有类型模板参数都yoghurt具体类型代表，所以<>里就空了
	struct TC<int, int>
	{
		TC()
		{
			cout << "TC<int,int>特化版本构造函数" << endl;
		}
		void functest1();
		/*{
			cout << "functest1特化版本" << endl;
		}*/

		void functest2();
		/*{
			cout << "functest2特化版本" << endl;
		}*/
	};
	//template <>
	void TC<int, int>::functest1()
	{
		cout << "functest1特化版本" << endl;
	}
	void TC<int, int>::functest2()
	{
		cout << "functest2特化版本" << endl;
	}

	//-------------------------
	template <typename U>
	struct TC<float, U>
	{
		TC()
		{
			cout << "TC<float,U>偏特化版本构造函数" << endl;
		}
		void functest1();
	};
	template <typename U>
	void  TC<float, U>::functest1()
	{
		cout << "TC<float,U>::functest1偏特化版本" << endl;
	}

	//---------范围偏特化-------------
	template <typename T, typename U>
	struct TC<const T, U*>
	{
		TC()
		{
			cout << "TC<const T,U*>偏特化版本构造函数" << endl;
		}
		void functest1();
	};
	template <typename T, typename U>
	void TC<const T, U*>::functest1()
	{
		cout << "TC<const T,U*>::functest1偏特化版本" << endl;
	}

}

int main()
{
	/***
	二：类模板的各种特化
	一般来讲，所写的类模板都是泛化的类模板
	特化的类模板是通过泛化的类模板来生成的，所以：得先要有泛化版本，才能有特化版本。
	所谓特化版本，就是特殊对待的版本

	2.1 类模板的全特化
	全特化：就是把TC这个泛化版本中的所有模板参数都用具体的类型来代替构成一个特殊的版本（全特化版本）。
	在理解上：泛化版本的类模板与全特化版本的类模板，只是名字相同（都叫TC），在其他方面，
	可以把实例化后的他们理解成是两个完全不同的类。

	2.2 普通成员函数的全特化

	2.3 静态成员变量的全特化
	特别值得一提的是：如果进行了普通成员函数的全特化，或者是静态成员变量的全特化，
	那么，就无法用这些全特化时指定的类型来对整个类模板进行全特化了。

	2.4 类模板的偏特化（局部特化）
	一方面：模板参数数量上的偏特化，另一方面是模板参数范围上的偏特化

	2.4.1 模板参数数量上的偏特化
	2.4.2 模板参数范围上的偏特化
	int ->const int.   T->T*  ,  T->T&    ,T->T&&

	***/

	/*_nmsp1::TC<int, float> mytc;
	mytc.functest1();

	_nmsp1::TC<int, int> mytc2;
	mytc2.functest1();
	mytc2.functest2();*/

	/*_nmsp1::TC<double, int> mytc3;
	mytc3.functest1();

	cout << "mytc3.m_stc = " << mytc3.m_stc << endl;*/

	//_nmsp1::TC<float, int> mytc4;
	//mytc4.functest1();

	_nmsp1::TC<const float, int *> mytc5;
	mytc5.functest1();

	return 0;
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Stereo/PyramidCorrelator.h>
#include <vw/Stereo/OptimizedCorrelator.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/FileIO/DiskImageResource.h>
#include <boost/foreach.hpp>

using namespace vw;
using namespace stereo;

std::vector<vw::BBox2i>
PyramidCorrelator::subdivide_bboxes(ImageView<PixelDisp > const& disparity_map,
                                    ImageView<PixelMask<uint8> > const& valid_pad,
                                    BBox2i const& box) {
  std::vector<BBox2i> result;
  BBox2i box_div_2 = box / 2;
  BBox2f disp_range;
  try {
    disp_range = get_disparity_range(crop(disparity_map, box_div_2));
  } catch (const std::exception& /*e*/) {
    // If there are no good pixels, don't add this box
    if (count_valid_pixels(crop(valid_pad, box_div_2)) == 0)
      return result;

    // Return a large bbox so that we keep dividing
    disp_range = BBox2f();
  }

  if (disp_range.width()*disp_range.height() <= 4 ||
      (box.width() < m_min_subregion_dim && box.height() < m_min_subregion_dim)) {
    // The bounding box is small enough.
    result.push_back(box);
    return result;
  } else {
    BBox2i subbox1 = box, subbox2 = box;
    if (box.width() > box.height()) {
      subbox1.max().x() = box.min().x() + box.width()/2;
      subbox2.min().x() = box.min().x() + box.width()/2;
    } else {
      subbox1.max().y() = box.min().y() + box.height()/2;
      subbox2.min().y() = box.min().y() + box.height()/2;
    }

    result = subdivide_bboxes(disparity_map, valid_pad, subbox1);
    std::vector<BBox2i> l2 = subdivide_bboxes(disparity_map, valid_pad, subbox2);
    result.insert(result.end(), l2.begin(), l2.end());
    return result;
  }
}

void draw_bbox(ImageView<PixelRGB<float> > &view, BBox2i const& bbox) {
  int32 u,v;
  // Top
  v = bbox.min().y();
  for (u = bbox.min().x(); u < bbox.max().x(); ++u)
    view(u,v) = vw::PixelRGB<float>(1.0,0.0,0.0);

  // Bottom
  v = bbox.max().y()-1;
  for (u = bbox.min().x(); u < bbox.max().x(); ++u)
    view(u,v) = vw::PixelRGB<float>(1.0,0.0,0.0);

  // Left
  u = bbox.min().x();
  for (v = bbox.min().y(); v < bbox.max().y(); ++v)
    view(u,v) = vw::PixelRGB<float>(1.0,0.0,0.0);

  // Left
  u = bbox.max().x()-1;
  for (v = bbox.min().y(); v < bbox.max().y(); ++v)
    view(u,v) = vw::PixelRGB<float>(1.0,0.0,0.0);
}

void PyramidCorrelator::write_debug_images(int32 n, ImageViewRef<PixelDisp> const& disparity_map,
                                           std::vector<BBox2i> const& nominal_blocks) {
  std::ostringstream current_level;
  current_level << n;
  BBox2f disp_range;
  try {
    disp_range = get_disparity_range(disparity_map);
  } catch (const std::exception& /*e*/) {
    // There was no good pixels
    disp_range = BBox2f();
  }
  ImageView<PixelRGB<float> > horz = normalize(clamp(select_channel(disparity_map,0), disp_range.min().x(), disp_range.max().x() ));
  ImageView<PixelRGB<float> > vert = normalize(clamp(select_channel(disparity_map,1), disp_range.min().y(), disp_range.max().y() ));

  for (size_t i = 0; i < nominal_blocks.size(); ++i) {
    draw_bbox(horz, nominal_blocks[i]);
    draw_bbox(vert, nominal_blocks[i]);
  }

  write_image( m_debug_prefix+current_level.str()+"-H.jpg", horz);
  write_image( m_debug_prefix+current_level.str()+"-V.jpg", vert);
}

// Iterate over the nominal blocks, creating output blocks for correlation
//
// To compute the block size, we must 1) Adjust the and size of
// the blocks for the right image to account for the disparity
// search range, 2) adjust the size of the left image blocks to
// be equal to the size of the right image blocks, and 3) buffer
// both blocks by the kernel size.
//
// Returns an updated search range that has been recentered for use in
// comparing the left_blocks and right_blocks.
BBox2f PyramidCorrelator::compute_matching_blocks(BBox2i const& nominal_block,
                                                BBox2f const& search_range,
                                                BBox2i &left_block, BBox2i &right_block) {

  left_block = nominal_block;

  // The bounds of the right box depend on the size of the requested search range.
  right_block = BBox2i(Vector2i(nominal_block.min().x()+int(floor(search_range.min().x())),
                                nominal_block.min().y()+int(floor(search_range.min().y()))),
                       Vector2i(nominal_block.max().x()+int(ceil(search_range.max().x())),
                                nominal_block.max().y()+int(ceil(search_range.max().y()))));

  // Ensure that the left and right blocks are the same size.
  left_block.max() = Vector2i(nominal_block.min().x() + right_block.width(),
                              nominal_block.min().y() + right_block.height());

  // Pad all blocks by the kernel size
  right_block.min() -= Vector2i(m_kernel_size[0], m_kernel_size[1]);
  right_block.max() += Vector2i(m_kernel_size[0], m_kernel_size[1]);
  left_block.min() -= Vector2i(m_kernel_size[0], m_kernel_size[1]);
  left_block.max() += Vector2i(m_kernel_size[0], m_kernel_size[1]);

  return BBox2f(0,0,ceil(search_range.width()),ceil(search_range.height()));
}

//  Use the previous level's disparity map to narrow the search range
//  for each nominal block. At each new level, we buffer the search
//  range slightly so that we cover all feasible disparity levels at
//  the higher level of resolution.
std::vector<BBox2f>
PyramidCorrelator::compute_search_ranges(ImageView<PixelDisp > const& prev_disparity_map,
                                         std::vector<BBox2i> const& nominal_blocks) {
  std::vector<BBox2f> search_ranges(nominal_blocks.size());
  std::vector<bool> is_good(nominal_blocks.size());

  // Step 1: compute the search ranges from the disparity map.
  for (size_t i = 0; i < nominal_blocks.size(); ++i) {
    ImageViewRef<PixelDisp > crop_disparity =
      crop(prev_disparity_map,(nominal_blocks[i])/2);
    if (count_valid_pixels(crop_disparity) > 20 * 20) {
      search_ranges[i] = get_disparity_range(crop_disparity);
      is_good[i]=true;
    } else {
      // Not enough good pixels available
      search_ranges[i] = BBox2f(0, 0, 0, 0);
      is_good[i]=false;
    }
  }

  // Step 2: adjust the search range or fix it if it came from a
  // block with zero valid pixels.
  std::list<size_t> was_corrected;
  for (size_t r = 0; r < nominal_blocks.size(); ++r) {

    // Pick a reasonable search range based on neighboring tiles rather
    // than skipping out entirely here.
    if ( !is_good[r] ) {
      // Find adjancent tiles and use the search ranges of adjacent
      // tiles to seed us with a reasonable search range of our own.
      //
      // To do this test, we expand this block by 1 pixel in every
      // direction and then test for intersections with other blocks.
      BBox2i this_bbox = nominal_blocks[r];
      this_bbox.expand(1);
      bool found_one_match = false;
      for (size_t b = 0; b < nominal_blocks.size(); ++b)
        if ( is_good[b] && this_bbox.intersects(nominal_blocks[b])) {
          if (found_one_match) {
            search_ranges[r].grow(search_ranges[b]);
          } else {
            found_one_match = true;
            was_corrected.push_back( r );
            search_ranges[r] = search_ranges[b];
          }
        }
    }
  }

  // Step 2 1/2: Mark good the search ranges that have been corrected
  BOOST_FOREACH( size_t const& i, was_corrected ) {
    is_good[i] = true;
  }

  // Step 3: scale up the search range for the next pyramid
  // level and pad it here.
  for (size_t r = 0; r < nominal_blocks.size(); ++r)
    if ( is_good[r] ) {
      search_ranges[r] *= 2;
      search_ranges[r].expand(2);
    }

  return search_ranges;
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_Estimator.hpp
//! \author Alex Robinson
//! \brief  Estimator base class declaration
//!
//---------------------------------------------------------------------------//

#ifndef FACEMC_ESTIMATOR_HPP
#define FACEMC_ESTIMATOR_HPP

// Std Lib Includes
#include <string>
#include <set>
#include <map>
#include <vector>

// Boost Includes
#include <boost/unordered_map.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_TwoDArray.hpp>
#include <Teuchos_ScalarTraits.hpp>
#include <Teuchos_any.hpp>
#include <Teuchos_Comm.hpp>

// FRENSIE Includes
#include "MonteCarlo_ParticleType.hpp"
#include "MonteCarlo_ResponseFunction.hpp"
#include "MonteCarlo_PhaseSpaceDimension.hpp"
#include "MonteCarlo_PhaseSpaceDimensionTraits.hpp"
#include "MonteCarlo_EstimatorDimensionDiscretization.hpp"
#include "MonteCarlo_ModuleTraits.hpp"
#include "MonteCarlo_EstimatorHDF5FileHandler.hpp"
#include "Utility_GlobalOpenMPSession.hpp"
#include "Utility_PrintableObject.hpp"
#include "Utility_Tuple.hpp"
#include "Utility_ContractException.hpp"

namespace MonteCarlo{

//! The estimator base class
class Estimator : public Utility::PrintableObject
{

public:

  //! Typedef for estimator id
  typedef ModuleTraits::InternalEstimatorHandle idType;

  //! Typedef for tuple of estimator moments (1st,2nd)
  typedef Utility::Pair<double,double> TwoEstimatorMoments;

  //! Typedef for tuple of estimator moments (1st,2nd,3rd,4th)
  typedef Utility::Quad<double,double,double,double> FourEstimatorMoments;

  //! Typedef for the array of estimator moments
  typedef Teuchos::Array<TwoEstimatorMoments> TwoEstimatorMomentsArray;

  //! Typedef for the array of estimator moments
  typedef Teuchos::Array<FourEstimatorMoments> FourEstimatorMomentsArray;

protected:

  //! Typedef for Teuchos::ScalarTraits
  typedef Teuchos::ScalarTraits<double> ST;

  // Typedef for map of dimension values
  typedef boost::unordered_map<PhaseSpaceDimension,Teuchos::any>
  DimensionValueMap;

public:

  //! Set the number of particle histories that will be simulated
  static void setNumberOfHistories( const unsigned long long num_histories );

  //! Set the start time for the figure of merit calculation
  static void setStartTime( const double start_time );

  //! Set the end time for the figure of merit calculation
  static void setEndTime( const double end_time );

  //! Constructor
  Estimator( const idType id,
	     const double multiplier );

  //! Destructor
  virtual ~Estimator()
  { /* ... */ }

  //! Return the estimator id
  idType getId() const;

  //! Set the bin boundaries for a dimension of the phase space (floating pt)
  template<PhaseSpaceDimension dimension, typename DimensionType>
  void setBinBoundaries( const Teuchos::Array<DimensionType>& bin_boundaries );

  //! Return the number of bins for a dimension of the phase space
  unsigned getNumberOfBins( const PhaseSpaceDimension dimension ) const;

  //! Return the total number of bins
  unsigned getNumberOfBins() const;

  //! Set the response functions
  virtual void setResponseFunctions(
   const Teuchos::Array<Teuchos::RCP<ResponseFunction> >& response_functions );

  //! Return the number of response functions
  unsigned getNumberOfResponseFunctions() const;

  //! Set the particle types that can contribute to the estimator
  virtual void setParticleTypes(
			  const Teuchos::Array<ParticleType>& particle_types );

  //! Check if the particle type is assigned to the estimator
  bool isParticleTypeAssigned( const ParticleType particle_type ) const;

  //! Check if the estimator has uncommitted history contributions
  bool hasUncommittedHistoryContribution( const unsigned thread_id ) const;

  //! Check if the estimator has uncommitted history contributions
  bool hasUncommittedHistoryContribution() const;

  //! Enable support for multiple threads
  virtual void enableThreadSupport( const unsigned num_threads );

  //! Commit the contribution from the current history to the estimator
  virtual void commitHistoryContribution() = 0;

  //! Reset estimator data
  virtual void resetData() = 0;

  //! Reduce estimator data on all processes in comm and collect on the root
  virtual void reduceData(
	    const Teuchos::RCP<const Teuchos::Comm<unsigned long long> >& comm,
	    const int root_process ) = 0;

  //! Export the estimator data
  virtual void exportData( EstimatorHDF5FileHandler& hdf5_file,
			   const bool process_data ) const;

protected:

  //! Set the has uncommited history contribution flag
  void setHasUncommittedHistoryContribution( const unsigned thread_id );

  //! Unset the has uncommited history contribution flag
  void unsetHasUncommittedHistoryContribution( const unsigned thread_id );

  //! Assign bin boundaries to an estimator dimension
  virtual void assignBinBoundaries(
	const Teuchos::RCP<EstimatorDimensionDiscretization>& bin_boundaries );

  //! Return the estimator constant multiplier
  double getMultiplier() const;

  //! Return the response function name
  const std::string& getResponseFunctionName(
				const unsigned response_function_index ) const;

  //! Return the name of the bin (with response function)
  std::string getBinName( const unsigned bin_index ) const;

  //! Print the estimator response function names
  void printEstimatorResponseFunctionNames( std::ostream& os ) const;

  //! Print the estimator bins
  void printEstimatorBins( std::ostream& os ) const;

  //! Print the estimator data stored in an array
  void printEstimatorBinData(
			 std::ostream& os,
			 const TwoEstimatorMomentsArray& estimator_moment_data,
			 const double norm_constant ) const;

  //! Print the total estimator data stored in an array
  void printEstimatorTotalData(
		 std::ostream& os,
		 const FourEstimatorMomentsArray& total_estimator_moments_data,
		 const double norm_constant ) const;

  //! Evaluate the desired response function
  double evaluateResponseFunction(
				const ParticleState& particle,
				const unsigned response_function_index ) const;

  //! Convert particle state to a generic map
  void convertParticleStateToGenericMap(
				   const ParticleState& particle,
				   const double angle_cosine,
				   DimensionValueMap& dimension_values ) const;

  //! Check if the point is in the estimator phase space
  bool isPointInEstimatorPhaseSpace(
		             const DimensionValueMap& dimension_values ) const;

  //! Calculate the bin index for the desired response function
  unsigned calculateBinIndex( const DimensionValueMap& dimension_values,
			      const unsigned response_function_index ) const;

  //! Calculate the response function index given a bin index
  unsigned calculateResponseFunctionIndex( const unsigned bin_index ) const;

  //! Convert first and second moments to mean and relative error
  void processMoments( const Utility::Pair<double,double>& moments,
		       const double norm_constant,
		       double& mean,
		       double& relative_error ) const;

  //! Convert first, second, third, fourth moments to mean, rel. er., vov, fom
  void processMoments(
		     const Utility::Quad<double,double,double,double>& moments,
		     const double norm_constant,
		     double& mean,
		     double& relative_error,
		     double& variance_of_variance,
		     double& figure_of_merit ) const;

private:

  // Convert a portion of the particle state to a generic map
  template<PhaseSpaceDimension dimension>
  void convertPartialParticleStateToGenericMap(
				   const ParticleState& particle,
			           DimensionValueMap& dimension_values ) const;

  // Calculate the mean of a set of contributions
  double calculateMean( const double first_moment_contributions ) const;

  // Calculate the relative error of a set of contributions
  double calculateRelativeError(
			      const double first_moment_contributions,
			      const double second_moment_contributions ) const;

  // Calculate the variance of the variance (VOV) of a set of contributions
  double calculateVOV( const double first_moment_contributions,
		       const double second_moment_contributions,
		       const double third_moment_contributions,
		       const double fourth_moment_contributions ) const;

  // Calculate the figure of merit (FOM) of an estimator bin
  double calculateFOM( const double relative_error ) const;

  // The tolerance used for relative error and vov calculations
  static double tol;

  // The number of particle histories that will be run
  static unsigned long long num_histories;

  // The start time used for the figure of merit calculation
  static double start_time;

  // The end time used for the figure of merit calculation
  static double end_time;

  // The estimator id
  idType d_id;

  // The constant multiplier for the estimator
  double d_multiplier;

  // Records if there is an uncommitted history contribution
  Teuchos::Array<unsigned char> d_has_uncommitted_history_contribution;

  // The response functions
  Teuchos::Array<Teuchos::RCP<ResponseFunction> > d_response_functions;

  // The estimator phase space dimension bin boundaries map
  boost::unordered_map<PhaseSpaceDimension,
  		       Teuchos::RCP<EstimatorDimensionDiscretization> >
  d_dimension_bin_boundaries_map;

  // The estimator phase space dimension index step size map
  boost::unordered_map<PhaseSpaceDimension,unsigned>
  d_dimension_index_step_size_map;

  // The estimator phase space dimension ordering
  Teuchos::Array<PhaseSpaceDimension> d_dimension_ordering;

  // The particle types that this estimator will take contributions from
  std::set<ParticleType> d_particle_types;
};

// Return the estimator id
inline Estimator::idType Estimator::getId() const
{
  return d_id;
}

// Return the estimator constant multiplier
inline double Estimator::getMultiplier() const
{
  return d_multiplier;
}

// Return the number of bins for a dimension of the phase space
inline unsigned Estimator::getNumberOfBins(
				    const PhaseSpaceDimension dimension ) const
{
  if( d_dimension_bin_boundaries_map.count( dimension ) != 0 )
    return d_dimension_bin_boundaries_map.find(dimension)->second->getNumberOfBins();
  else
    return 1u;
}

// Return the total number of bins
inline unsigned Estimator::getNumberOfBins() const
{
  unsigned number_of_bins = 1u;

  for( unsigned i = 0u; i < d_dimension_ordering.size(); ++i )
    number_of_bins *= getNumberOfBins( d_dimension_ordering[i] );

  return number_of_bins;
}

// Return the number of response functions
inline unsigned Estimator::getNumberOfResponseFunctions() const
{
  return d_response_functions.size();
}

// Check if the particle type is assigned to the estimator
inline bool Estimator::isParticleTypeAssigned(
					const ParticleType particle_type) const
{
  return d_particle_types.count( particle_type );
}

// Return the response function name
inline const std::string& Estimator::getResponseFunctionName(
				 const unsigned response_function_index ) const
{
  // Make sure the response function index is valid
  testPrecondition( response_function_index < getNumberOfResponseFunctions() );

  return d_response_functions[response_function_index]->getName();
}

// Evaluate the desired response function
inline double Estimator::evaluateResponseFunction(
				 const ParticleState& particle,
				 const unsigned response_function_index ) const
{
  // Make sure the response function index is valid
  testPrecondition( response_function_index < getNumberOfResponseFunctions() );

  return d_response_functions[response_function_index]->evaluate( particle );
}

// Convert particle state to a generic map
inline void Estimator::convertParticleStateToGenericMap(
			      const ParticleState& particle,
			      const double angle_cosine,
			      DimensionValueMap& dimension_values ) const
{
  // It may be useful to neglect the COSINE_DIMENSION in the future...
  convertPartialParticleStateToGenericMap<DIMENSION_start>( particle,
							    dimension_values );

  // Assign the cosine dimension value separately
  dimension_values[COSINE_DIMENSION] =
    PhaseSpaceDimensionTraits<COSINE_DIMENSION>::obfuscateValue(
								angle_cosine );
}

// Check if the estimator has uncommitted history contributions
inline bool Estimator::hasUncommittedHistoryContribution() const
{
  return hasUncommittedHistoryContribution(
				 Utility::GlobalOpenMPSession::getThreadId() );
}

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "MonteCarlo_Estimator_def.hpp"

//---------------------------------------------------------------------------//

#endif // end FACEMC_ESTIMATOR_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_Estimator.hpp
//---------------------------------------------------------------------------//


//
// $Id$
//
// The contents of this file are subject to the Mozilla Public License
// Version 1.1 (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.mozilla.org/MPL/
//
// Software distributed under the License is distributed on an "AS IS"
// basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
// License for the specific language governing rights and limitations
// under the License.
//
// The Original Code is the DirecTag peptide sequence tagger.
//
// The Initial Developer of the Original Code is Zeqiang Ma.
//
// Copyright 2009 Vanderbilt University
//
// Contributor(s):
//

// adjust ScanRanker scores by groups
// use mean and IQR of all files in a group for score normalization
// input: ScanRanker metrics files separated by comma, one line for one group

#include <stdio.h>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>

using boost::lexical_cast;
using namespace std;

struct ScoreInfo
{
    float    bestTagScoreMean;
    float    bestTagTICMean;
    float    tagMzRangeMean;
    float    bestTagScoreIQR;
    float    bestTagTICIQR;
    float    tagMzRangeIQR;
    int        numTaggedSpectra;
};

///    <summary>
///    extract ScanRanker subscore information from a metrics file
///    </summary>
void getHeader(const string& filename, ScoreInfo *scoreInfo )
{
    // Open the input file
    ifstream fileStream( filename.c_str() );
    if( !fileStream.is_open() )
        throw invalid_argument( string( "unable to open file \"" ) + filename + "\"" );

    string headerLine;
    getline(fileStream, headerLine); //discard the first header line: H    BestTagScoreMean    BestTagTICMean    TagMzRangeMean    BestTagScoreIQR    BestTagTICIQR    TagMzRangeIQR    numTaggedSpectra
    getline(fileStream, headerLine); // get second line, e.g.: H    25.4616    7.06014    701.679    12.0654    4.52176    305.127    18587
    stringstream lineStream(headerLine);
    string segments;
    vector<string> v;
    while(getline(lineStream,segments,'\t'))
    {
        v.push_back(segments);
    }
//    copy(v.begin(), v.end(), ostream_iterator<string>(cout, "\n"));

    scoreInfo->bestTagScoreMean = lexical_cast<float>( v[1] );
    scoreInfo->bestTagTICMean = lexical_cast<float>( v[2] );
    scoreInfo->tagMzRangeMean = lexical_cast<float>( v[3] );
    scoreInfo->bestTagScoreIQR = lexical_cast<float>( v[4] );
    scoreInfo->bestTagTICIQR = lexical_cast<float>( v[5] );
    scoreInfo->tagMzRangeIQR = lexical_cast<float>( v[6] );
    scoreInfo->numTaggedSpectra = lexical_cast<int>( v[7] );
}

///    <summary>
///    adjust scores with global mean and IQR of a group, write new metrics file
///    </summary>
void adjustScore(    const string& filename,
                    float    gbBestTagScoreMean,
                    float    gbBestTagTICMean,
                    float    gbTagMzRangeMean,
                    float    gbBestTagScoreIQRMean,
                    float    gbBestTagTICIQRMean,
                    float    gbTagMzRangeIQRMean)
{
    cout << '\t' << filename << '\n';
    namespace bfs = boost::filesystem;
    bfs::path newFilename = bfs::basename(filename) + "-adjusted.txt";
     string outputFilename = newFilename.string();
    ofstream ofileStream( outputFilename.c_str() );

    // Open the input file
    ifstream fileStream( filename.c_str() );
    if( !fileStream.is_open() )
        throw invalid_argument( string( "unable to open file \"" ) + filename + "\"" );

    string line;
    while( getline(fileStream, line))
    {
        if (line.find("H\tBestTagScoreMean") != string::npos)
        {
            ofileStream << "H\tBestTagScoreMean\tBestTagTICMean\tTagMzRangeMean\tBestTagScoreIQR\tBestTagTICIQR\tTagMzRangeIQR\tnumTaggedSpectra\tgbBestTagScoreMean\tgbBestTagTICMean\tgbTagMzRangeMean\tgbBestTagScoreIQRMean\tgbBestTagTICIQRMean\tgbTagMzRangeIQRMean\n";
        }
        else if (line.find("H\tIndex") != string::npos)    // if header line, write out line, else extract subscores and compute the new score
        {
            ofileStream << line << "\tAdjustedScore\n";
        }
        else if (line.find("H\t") != string::npos)
        {
            ofileStream << line << '\t'
                        << gbBestTagScoreMean << '\t'
                        << gbBestTagTICMean << '\t'
                        << gbTagMzRangeMean << '\t'
                        << gbBestTagScoreIQRMean << '\t'
                        << gbBestTagTICIQRMean << '\t'
                        << gbTagMzRangeIQRMean << '\n';

        }
        else
        {
            stringstream lineStream(line);
            string segments;
            vector<string> v;
            while(getline(lineStream,segments,'\t'))
            {
                v.push_back(segments);
            }
            // metrics file format:
            //H    NativeID    PrecursorMZ    Charge    PrecursorMass    BestTagScore    BestTagTIC    TagMzRange    ScanRankerScore
            float bestTagScore = lexical_cast<float>( v[5] );
            float bestTagTIC = lexical_cast<float>( v[6] );
            float tagMzRange = lexical_cast<float>( v[7] );
            float originalScore = lexical_cast<float>( v[8] );
            float bestTagScoreNorm = ( bestTagScore - gbBestTagScoreMean ) / gbBestTagScoreIQRMean;
            float bestTagTICNorm = ( bestTagTIC - gbBestTagTICMean ) / gbBestTagTICIQRMean;
            float tagMzRangeNorm = ( tagMzRange - gbTagMzRangeMean ) / gbTagMzRangeIQRMean;
            float adjustedScore = ( bestTagScoreNorm + bestTagTICNorm + tagMzRangeNorm) / 3;

            ofileStream << line << '\t' << adjustedScore << '\n';
        }
    }
}

int main( int argc, char* argv[] )
{
    if (argc < 2)
    {
        cerr << "Not enough arguments.\n" << "Usage: " << argv[0] << "   groupFile" << endl;
        return 1;
    }

    ifstream  groupFile( argv[1] );
    string line;
    while(getline(groupFile,line))  //work on each group; one line one group, separated with ","
    {
        stringstream  lineStream(line);
        string        file;
        vector<ScoreInfo> scoreInfoVector;
        //cout << "Extracting subscore infomation from metrics files ... \n" << endl;
        while(getline(lineStream,file,','))  //work on a sigle file in a group, extract mean and IQR from each file
        {
            ScoreInfo scoreInfo;
            getHeader( file, &scoreInfo );
            scoreInfoVector.push_back( scoreInfo );
        }

        // iterate scoreInforVector, compute global mean and IQR for normalization
        float    gbBestTagScoreSum = 0.0;
        float    gbBestTagTICSum = 0.0;
        float    gbTagMzRangeSum = 0.0;
        float    gbBestTagScoreIQRSum = 0.0;
        float    gbBestTagTICIQRSum = 0.0;
        float    gbTagMzRangeIQRSum = 0.0;
        int        totalSpectra = 0;

        for( vector<ScoreInfo>::iterator itr = scoreInfoVector.begin(); itr != scoreInfoVector.end(); ++itr )
        {
            gbBestTagScoreSum += itr->bestTagScoreMean * itr->numTaggedSpectra ;
            gbBestTagTICSum += itr->bestTagTICMean * itr->numTaggedSpectra ;
            gbTagMzRangeSum += itr->tagMzRangeMean * itr->numTaggedSpectra ;
            gbBestTagScoreIQRSum += itr->bestTagScoreIQR ;
            gbBestTagTICIQRSum += itr->bestTagTICIQR ;
            gbTagMzRangeIQRSum += itr->tagMzRangeIQR ;
            totalSpectra +=  itr->numTaggedSpectra ;
        }

        int numFiles = (int) scoreInfoVector.size();
        float gbBestTagScoreMean = gbBestTagScoreSum / (float) totalSpectra;
        float gbBestTagTICMean = gbBestTagTICSum / (float) totalSpectra;
        float gbTagMzRangeMean = gbTagMzRangeSum / (float) totalSpectra;
        float gbBestTagScoreIQRMean = gbBestTagScoreIQRSum / (float) numFiles;
        float gbBestTagTICIQRMean = gbBestTagTICIQRSum / (float) numFiles;
        float gbTagMzRangeIQRMean = gbTagMzRangeIQRSum / (float) numFiles;

        lineStream.clear(); // reset lineStream for getline()
        lineStream.seekg(0,std::ios::beg);
        cout << "Adjusting ScanRanker scores ...\n" << endl;
        while(getline(lineStream,file,','))  //work on a sigle file in a group, add adjusted scores
        {
            adjustScore(    file,
                            gbBestTagScoreMean,
                            gbBestTagTICMean,
                            gbTagMzRangeMean,
                            gbBestTagScoreIQRMean,
                            gbBestTagTICIQRMean,
                            gbTagMzRangeIQRMean);
        }

    }
}


#include <boost/python.hpp>
#include <numpy/arrayobject.h>
#include <numpy_eigen.h>
#include <omp.h>

#include "integrand.h"

using namespace Eigen;

PyObject* calc_Gavg(PyObject *pyw, const double &delta, const double &mu,
    PyObject *pySE, PyObject *pyTB, const double &Hf, PyObject *py_bp,
    PyObject *py_wf, const int nthreads) {
  try {
    if (nthreads > 0) omp_set_num_threads(nthreads);
    MatrixXcd SE;
    VectorXcd w;
    VectorXd bp, wf;
    VectorXd SlaterKosterCoeffs;

    numpy::from_numpy(pySE, SE);
    numpy::from_numpy(pyTB, SlaterKosterCoeffs);
    numpy::from_numpy(py_bp, bp);
    numpy::from_numpy(py_wf, wf);
    numpy::from_numpy(pyw, w);
    assert(w.size() == SE.rows());

    double t = SlaterKosterCoeffs(0);
    VectorXd xl(DIM), xh(DIM);
    xl << -2*t;
    xh << 2*t;
    double BZ1 = 1.;

    MatrixXcd result(SE.rows(), MSIZE*MSIZE);
    VectorXcd tmp(MSIZE);
    GreenIntegrand green_integrand(w, mu, SE, SlaterKosterCoeffs, Hf);
    result.setZero();
    for (int n = 0; n < w.size(); ++n) {
      green_integrand.set_data(n);
      tmp = md_int::Integrate(xl, xh, green_integrand, bp, wf);
      for (int i = 0; i < MSIZE; ++i)
        result(n, MSIZE*i + i) = tmp(i);
    }
    result /= BZ1;
    return numpy::to_numpy(result);
  } catch (const char *str) {
      std::cerr << str << std::endl;
      return Py_None;
    }
}

PyObject* calc_Havg(const int &Norder, PyObject *pyTB, const double &Hf,
    const double &delta, PyObject *py_bp, PyObject *py_wf) {
  try {
    MatrixXi R;
    VectorXd bp, wf;
    VectorXd SlaterKosterCoeffs;

    numpy::from_numpy(pyTB, SlaterKosterCoeffs);
    numpy::from_numpy(py_bp, bp);
    numpy::from_numpy(py_wf, wf);

    double t = SlaterKosterCoeffs(0);
    VectorXd xl(DIM), xh(DIM);
    xl << -2*t;
    xh << 2*t;
    double BZ1 = 1.;

    HIntegrand h_integrand(Norder, SlaterKosterCoeffs, Hf);
    VectorXd result = (md_int::Integrate(xl, xh, h_integrand, bp, wf)).real();
    result /= BZ1;
    VectorXd ret(Norder*MSIZE*MSIZE);
    ret.setZero();
    for (int i = 0; i < Norder; ++i)
      for (int j = 0; j < MSIZE; ++j)
        ret(MSIZE*MSIZE*i + MSIZE*j + j) = result(MSIZE*i + j);
    return numpy::to_numpy(ret);
  } catch (const char *str) {
    std::cerr << str << std::endl;
    return Py_None;
  }
}

BOOST_PYTHON_MODULE(int_bethe_lattice)
{
  using namespace boost::python;
  def("calc_Gavg", calc_Gavg);
  def("calc_Havg", calc_Havg);
}