#include <Rcpp.h>
#include <boost/multi_array.hpp> // 多次元配列用

using namespace Rcpp;

// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(BH)]]
//'Estimation of population distribution via EM algorithm.
//'
//'This function is new version of \code{MML_EM_dist}
//'@param x Item response matrix.
//'@param para item parameter data.frame estimated by \code{\link{estip}}
//'@param N the number of nodes in integration.
//'@param fc0 a column of first item response.
//'@param ng the number of groups
//'@param gc0 a column of group. the element must be integer and the minimum number must be 1.
//'@param eMLL a convergence criteria(CC) for marginal log likelihood.
//'@param eDIST a CC for population distribution.
//'@param D factor constant.
//'@param fix Don't use. If 1, fix population distribution mean and sigma each EM cycle.
//'@param print How much information you want to display? from 1 to 3. The larger, more information is displayed.
//'@param max maximum value of theta in integration.
//'@param min minimum value of theta in integration.
//'@param maxiter_em maximum iteration time for EM cycle.
//'@param rm_list a vector of item U want to remove for estimation. NOT list.
//'@export
// [[Rcpp::export]]

List estdist(DataFrame x, DataFrame para, const int N = 31, const double eMLL = 1e-6, const double eDIST = 1e-4,
             int fc0 = 2,int ng = 1, int gc0 = 2, const double D = 1.702, const int fix = 1, const int print = 0,
             const double max = 4.0, const double min = -4.0, const int maxiter_em = 200,
             CharacterVector rm_list = CharacterVector::create("NONE")
){
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // Update Information
  //
  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  struct LocalFunc{ // R function define

    static NumericVector colSums_cpp(NumericMatrix mat){ // R::colSums
      Function f("colSums");
      return f(mat, _["na.rm"] = true);
    }

    static NumericVector rowSums_cpp(NumericMatrix mat){ // R::rowSums
      Function f("rowSums");
      return f(mat, _["na.rm"] = true);
    }

    static NumericVector cor_cpp(NumericVector vec, NumericMatrix mat){ //R::cor
      Function f("cor");
      return f(vec, _["y"] = mat, _["use"] = "pairwise.complete.obs");
    }

    static NumericVector colMeans_cpp(NumericMatrix mat){ // R::colMeans
      Function f("colMeans");
      return f(mat, _["na.rm"] = true);
    }
  };

  const int lc = x.length();// item n
  const int fc = fc0 -1;// Rcpp上では0から要素数がカウントされるので，数値を調整する。

  // 入力データの確認
  const int nj = lc - fc;
  const int ni = x.nrows(); // subject n
  int rm_n = 0;
  if(rm_list[0] != "NONE") rm_n = rm_list.length();  // 削除項目

  const int gc = gc0 -1;

  IntegerVector group (ni,1);
  if(ng != 1) group = x[gc];
  //NumericVector group = x[gc];

  group = group -1; // 集団変数を，C++の要素数に適合するように，-1する。

  NumericMatrix xall (ni, nj);
  CharacterVector Item_temp = x.names();
  CharacterVector Item (nj);
  for (int j=0; j<nj; j++){ // もとのデータフレームから項目反応パタンだけを抜き出し，行列として保存
    int jj = j + fc;
    NumericVector kk = x[jj];
    Item[j] = Item_temp[jj];
    for(int i=0; i<ni; i++){
      double k = kk[i];
      xall(i,j) = k;
    }
  }
  // 削除した項目の列番号（データフレーム）を取得
  IntegerVector rm_id(1,9999);

  if(rm_list[0] != "NONE"){
    Rcout<<"You select Item; "<<rm_list<<"as not use.\n";
    rm_id = IntegerVector(rm_n); // result
    for(int l=0; l<rm_n; l++){
      String r = rm_list[l];
      rm_id[l] = x.findName(r)-(fc-1);
    }
  } else {

  }

  // 欠測値がある箇所を0，そうではない箇所を1とした行列を作成
  NumericMatrix resp (ni,nj);
  for(int i=0; i<ni; i++){
    for(int j=0; j<nj; j++){
      if(NumericVector::is_na(xall(i,j))) resp(i,j) = 0; // NAならば0を
      else resp(i,j) = 1; // NA以外（0か1）ならば1を
    }
  }

  NumericMatrix ind (ng,nj); // 当該集団が，項目に回答したか否か
  for(int g=0; g<ng; g++){
    // 集団が受検した項目を確認
    for(int i=0; i<ni; i++){
      if(group[i] != g) continue;
      for(int j=0; j<nj; j++){
        int q = ind(g,j);
        ind(g,j)= q + resp(i,j);
      }
    }

    for( int j=0; j<nj; j++){
      int q = ind(g,j);
      if(q != 0) ind(g,j) = 1; // 誰か一人でも回答していたら1，集団内が全員無回答なら0
    }
  }

  Rcout << "The number of subject is " << ni << ".\nThe number of item is " << nj-rm_n <<".\nThe number of remove item is "<<rm_n<<".\n";

  // initial values
  NumericMatrix t0(nj,3);
  NumericVector a0 = para["a"];
  NumericVector b0 = para["b"];
  NumericVector c0 = para["c"];
  // NumericVector a0 = para[0];
  // NumericVector b0 = para[1];
  // NumericVector c0 = para[2];
  for(int j=0; j<nj; j++){
    t0(j,0) = a0[j];
    t0(j,1) = b0[j];
    t0(j,2) = c0[j];
  }

  // prior distribution
  double seq = (max - min)/(N-1); // 分点の間隔。これは全母集団で固定。
  NumericVector Xm (N);
  Xm[0] = min;
  for(int m=1; m<N; m++){
    double pre = Xm[m-1];
    Xm[m] = pre + seq;
  }

  // 母集団分布の初期値を決定。
  NumericMatrix Um (N,ng); // 推定母集団分布計算用
  NumericMatrix dist (0);

  // 結果代入用の行列，ベクトルを準備　＝　こうしないと，最終的にスコープできない。
  boost::multi_array <double, 3> Gim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Lim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Nm (boost::extents[ng][nj][N]);
  boost::multi_array <double, 3> rjm (boost::extents[ng][nj][N]);
  NumericMatrix skip_para (nj,3); // 推定に失敗した項目番号の代入用

  /////////////////////////
  // EM step starts here //
  /////////////////////////
  // 推定母集団分布の計算
  // 初期値に一様分布をもちいた場合，Easy Estimationの計算結果とおおむね一致するはず。

  Rprintf("Start calculating estimated population distribution.\n");

  // result
  NumericVector mean_pop(ng);
  NumericVector sd_pop(ng);
  double diff2 = 0;
  NumericVector MLL2 (0);

  // 一様分布を初期値に設定する。
  NumericMatrix unif_dist (0);
  double NN = Um.nrow();
  for(int i=0; i<N; i++){
    for(int j=0; j<ng; j++){
      Um(i,j) = 1/NN;
    }
  }

  // start loop
  int count3 =0;
  int conv3 = 0;
  LogicalVector conv0(ng);

  while(conv3 == 0){
    count3 += 1;

    // calculate Lim
    double a,b,c,t,xx,tt,u;
    int g,i,m,j;
    for(g=0; g<ng; g++){
      for(i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        for(m=0; m<N; m++){
          t = 1;
          xx = Xm[m];
          tt = 0;
          for(j=0; j<nj; j++){
            //if(resp(i,j)==0) continue; // NAの反応パタンのところは計算ループから外れる。
            a = t0(j,0);
            if(a==0)continue;
            b = t0(j,1);
            c = t0(j,2);
            u = xall(i,j);
            if(u == 1){ // 正答した場合の尤度
              tt = c+(1.0-c)/(1.0+exp(-D*a*(xx-b)));
            } else if (u == 0) { // 誤答した場合の尤度
              tt = 1.0-(c+(1.0-c)/(1.0+exp(-D*a*(xx-b))));
            } else { // 欠測値の場合は尤度を計算しないので，1
              tt = 1;
            }
            t = t * tt; // sum
          }
          //boost::multi_array<double, 3>::index idx = {{g,i,m}};
          Lim[g][i][m] = t;
        }
      }
    }

    //Rcout<<"marginal log likelihood calculation.\n";
    // Calculation marginal log likelihood

    double f = 0; // 周辺対数尤度代入用
    double ff,fff,l,w;

    for(int g=0; g<ng; g++){
      for(int i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        ff = 0; // 受検者iの周辺対数尤度代入用
        for(int m=0; m<N; m++){
          l = Lim[g][i][m];
          w = Um(m,g);
          fff =  l*w ;
          ff = ff + fff;
        }
        f = f + log(ff);
      }
    }

    MLL2.push_back(f);
    diff2 = fabs(MLL2[count3-1] - MLL2[count3]); // 周辺対数尤度の差

    //Rcout << "-2 Marginal Log Likelihood is " << -2 * f <<"\r";
    Rprintf("%d times -2 Marginal Log Likelihood is %f \r",count3,-2*f); // 小数点形式の出力に対応

    // calculate Gim
    double uu;
    for(int g=0; g<ng; g++){
      for(int i=0; i<ni; i++){
        if(group[i] != g) continue; // 集団に属さない受験者の部分はスキップ
        u = 0;
        for(int m=0; m<N; m++){ // 総和を1にするための分母の計算 // sum
          l = Lim[g][i][m];
          w = Um(m,g);
          uu = l*w;
          u = u + uu;
        }
        for(int m=0; m<N; m++){
          l = Lim[g][i][m];
          w = Um(m,g);
          Gim[g][i][m] =  l * w / u;
        }
      }
    }

    // calculate Nm

    //Rcout<<"expected frequency of subjects in each nodes calculation.\n";
    double k,kk;
    for(int g=0; g<ng; g++){
      for(int j=0; j<nj; j++){ // 各分点の期待度数
        if(ind(g,j) == 0) continue;
        for(int m=0; m<N; m++){
          k = 0;
          for(int i=0; i<ni; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
            if(resp(i,j)==0) continue;
            //double d = resp(i,j);
            kk = Gim[g][i][m];
            k = k + kk; //* d;
          }
          Nm[g][j][m] = k;
        }
      }
    }

    // 一様分布の確率密度をリセット
    for(int m=0; m<N; m++){
      for(int g=0; g<ng; g++){
        Um(m,g) = 0;
      }
    }

    // 母集団ごとの平均と標準偏差を計算
    for(int g=0; g<ng; g++){
      // probability
      double mean_g = 0;
      double sd_g = 0;
      //int counti = 0; // 母集団の受検した項目数カウント用
      for(int j=0; j<nj; j++){
        // 集団内のだれも回答をしていなかったら，次の項目のループへスキップ。
        if(ind(g,j) == 0) continue;

        //counti = counti + 1;
        double mean_j = 0;
        double sNjm = 0; // 項目ごとの分点の受験者の期待度数の総和代入用
        for(int m=0; m<N; m++){  // sum
          double Njm = Nm[g][j][m];
          sNjm = sNjm + Njm;
        }

        for(int m=0; m<N; m++){ // sum
          double Njm = Nm[g][j][m];
          double xm = Xm[m];
          double tempM = xm * Njm/sNjm; // E(X) = \Sigma(x_i * prob_i)
          Um(m,g) += Njm/sNjm;
          mean_j += tempM; //
        } // end of m
        mean_g = mean_g + mean_j; // 項目ごとの母集団平均を足し上げたもの

        // sd

        double var_j = 0;
        double sd_j = 0;
        for(int m=0; m<N; m++){
          double Njm = Nm[g][j][m];
          double xm = Xm[m];
          double tempV = (xm -mean_j)*(xm -mean_j)*Njm/sNjm; // V(X) = Sigma(x_i-E(X))^2*prob_i
          var_j = var_j + tempV;
        }// end of m
        sd_j = std::sqrt(var_j); // 項目ごとに計算した標準偏差
        sd_g = sd_g + sd_j; // 項目ごとの母集団標準偏差を足し上げたもの

      } // end of j

      int counti = sum(ind(g,_));
      // 収束判定用
      if(fabs(mean_pop[g] - mean_g/counti)<eDIST && fabs(sd_pop[g] - sd_g/counti)<eDIST ) conv0[g] = 1;

      mean_pop[g] = mean_g/counti; // 全項目における母集団平均の，平均
      sd_pop[g] = sd_g/counti; // 全項目における母集団標準偏差の，平均
      if(print >= 1) Rcout << "mean: "<< mean_pop[g]<<", sd: "<<sd_pop[g]<<"\n";

      for(int m=0; m<N; m++){
        Um(m,g) /= counti;
      }

    } // end of g

    if(diff2 < eMLL || is_true(all(conv0)) ) conv3 = 1;
    //break;
  }

  unif_dist = cbind(Xm,Um);

  List res = List::create(_["population_dist"] = unif_dist, _["population_mean"] = mean_pop, _["population_sd"] = sd_pop,
                          _["MLL"] = MLL2, _["conv"] = conv3, _["count"] = count3,
                          _["skip_para"] = skip_para, _["rm_n"] = rm_n, _["rm_id"] = rm_id
  );

  return res;

}


/*! @file wavelet_filter_bank.cc
 *  @brief Wavelet filter bank tree (mid level).
 *  @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/primitives/wavelet_filter_bank.h"
#include <cassert>
#include <cstring>
#include <algorithm>
#include <list>
#include <memory>
#include <string>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#include <boost/regex.hpp>  // NOLINT(build/include_order)
#pragma GCC diagnostic pop
#include <simd/wavelet.h>
#include "src/stoi_function.h"

namespace sound_feature_extraction {

TreeFingerprint Parse(const std::string& str, identity<TreeFingerprint>) {
  static const boost::regex all_regex("^\\s*(\\d+\\s*(\\s|$))+");
  static const boost::regex int_regex("(\\d+\\s*(\\s|$))");

  boost::smatch match;
  if (!boost::regex_match(str, match, all_regex)) {
    throw WaveletTreeDescriptionParseException(str);
  }

  TreeFingerprint res;
  try {
    std::transform(boost::sregex_token_iterator(str.begin(), str.end(),
                                                int_regex,
                                                1),
                   boost::sregex_token_iterator(),
                   std::back_inserter(res),
                   std::stoi_function);
  }
  catch(const std::invalid_argument& e) {
    throw WaveletTreeDescriptionParseException(str);
  }
  if (res.empty()) {
    throw WaveletTreeDescriptionParseException(str);
  }
  return res;
}

WaveletType Parse(const std::string& value, identity<WaveletType>) {
  static const std::unordered_map<std::string, WaveletType> map = {
    { WAVELET_TYPE_DAUBECHIES_STR, WAVELET_TYPE_DAUBECHIES },
    { WAVELET_TYPE_COIFLET_STR, WAVELET_TYPE_COIFLET },
    { WAVELET_TYPE_SYMLET_STR, WAVELET_TYPE_SYMLET }
  };
  auto it = map.find(value);
  if (it == map.end()) {
    throw WaveletTreeWaveletTypeParseException(value);
  }
  return it->second;
}

namespace primitives {

WaveletFilterBank::WaveletFilterBank(WaveletType type, int order,
                                     const TreeFingerprint& treeDescription)
    : type_(type),
      order_(order),
      tree_(treeDescription) {

  ValidateDescription(treeDescription);
}

WaveletFilterBank::WaveletFilterBank(WaveletType type, int order,
                                     TreeFingerprint&& treeDescription)
    : type_(type),
      order_(order),
      tree_(treeDescription) {
  ValidateDescription(treeDescription);
}

void WaveletFilterBank::ValidateWavelet(WaveletType type,
                                      int order) {
  if (!wavelet_validate_order(type, order)) {
    throw WaveletTreeInvalidOrderException(type, order);
  }
}

void WaveletFilterBank::ValidateDescription(
    const TreeFingerprint& treeDescription) {
  if (treeDescription.size() < 2) {
    throw WaveletTreeInvalidDescriptionException(treeDescription);
  }
  std::list<int> tree(treeDescription.begin(), treeDescription.end());
  while (!tree.empty()) {
    // Reduce the tree using a simple grammar:
    // (n) (n) -> (n - 1)
    bool reduced = false;
    for (auto it = tree.begin(); it != tree.end(); it++) {
       auto next = it;
       next++;
       if (next != tree.end() && *it == *next) {
         if (*it == 1 && tree.size() == 2) {
           tree.erase(it, tree.end());
         } else {
           *it = *it - 1;
           tree.erase(next);
         }
         reduced = true;
         break;
       }
    }
    if (!reduced) {
      throw WaveletTreeInvalidDescriptionException(treeDescription);
    }
  }
}

void WaveletFilterBank::ValidateLength(
    const TreeFingerprint& tree, size_t length) {
  int max = *std::max_element(tree.begin(), tree.end());
  // length / 2^max >= 1
  if (length == 0 || length % (1 << max) != 0) {
    throw WaveletTreeInvalidSourceLengthException(tree, length);
  }
}

void WaveletFilterBank::Apply(const float* source, size_t length,
                              float *result) noexcept {
  assert(source && result);
  ValidateLength(tree_, length);

  // Support zero-copy
  auto lsrc =
#ifndef __AVX__
      (source == result)? result :
#endif
      wavelet_prepare_array(order_, source, length);
  ApplyInternal(lsrc, length, result);
#ifndef __AVX__
  if (source != result) {
#endif
  free(lsrc);
#ifndef __AVX__
  }
#endif
}

void WaveletFilterBank::ApplyInternal(float* source, size_t length,
                                      float *result) noexcept {
  assert(source && result);
  ValidateLength(tree_, length);

  TreeFingerprint tree(tree_);
  std::reverse(tree.begin(), tree.end());
  TreeFingerprint workingTree;
  workingTree.reserve(tree.size());

  auto ldesthi = std::unique_ptr<float, decltype(&std::free)>(
      wavelet_allocate_destination(order_, length), free);
  auto ldestlo = std::unique_ptr<float, decltype(&std::free)>(
      wavelet_allocate_destination(order_, length), free);
  wavelet_apply(type_, order_, EXTENSION_TYPE_PERIODIC, source, length,
                ldesthi.get(), ldestlo.get());
  float *desthihi, *desthilo, *destlohi, *destlolo;
  wavelet_recycle_source(order_, source, length,
                         &desthihi, &desthilo,
                         &destlohi, &destlolo);
  workingTree.push_back(1);
  workingTree.push_back(1);

  while (!tree.empty()) {
    RecursivelyIterate(type_, order_, length / 2, &tree, &workingTree,
                       ldesthi.get(), desthihi, desthilo, &result);
    RecursivelyIterate(type_, order_, length / 2, &tree, &workingTree,
                       ldestlo.get(), destlohi, destlolo, &result);
  }
}

void WaveletFilterBank::RecursivelyIterate(
    WaveletType type, int order, size_t length,
    TreeFingerprint *tree, TreeFingerprint* workingTree, float* source,
    float* desthi, float* destlo, float** result) noexcept {
  if (tree->back() != workingTree->back()) {
    wavelet_apply(type, order, EXTENSION_TYPE_PERIODIC, source, length, desthi,
                  destlo);
    float *desthihi, *desthilo, *destlohi, *destlolo;
    wavelet_recycle_source(order, source, length,
                           &desthihi, &desthilo,
                           &destlohi, &destlolo);
    int next = workingTree->back() + 1;
    workingTree->pop_back();
    workingTree->push_back(next);
    workingTree->push_back(next);
    RecursivelyIterate(type, order, length / 2, tree, workingTree,
                       desthi, desthihi, desthilo, result);
    RecursivelyIterate(type, order, length / 2, tree, workingTree,
                       destlo, destlohi, destlolo, result);
  } else {
    memcpy(*result, source, length * sizeof(source[0]));
    *result += length;
    tree->pop_back();
    workingTree->pop_back();
  }
}

}  // namespace primitives
}  // namespace sound_feature_extraction


/// @file
///
/// A linear solver for parameters from the normal equations
///
/// @copyright (c) 2007 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution 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 2 of the License,
/// or (at your option) any later version.
///
/// This program 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 this program; if not, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Tim Cornwell <tim.cornwell@csiro.au>
/// @author Vitaliy Ogarko <vogarko@gmail.com>
///
#ifdef HAVE_MPI
#include <mpi.h>
#endif

#include <fitting/LinearSolver.h>
#include <fitting/GenericNormalEquations.h>

#include <askap/AskapError.h>
#include <profile/AskapProfiler.h>
#include <boost/config.hpp>

#include <casacore/casa/aips.h>
#include <casacore/casa/Arrays/Array.h>
#include <casacore/casa/Arrays/Matrix.h>
#include <casacore/casa/Arrays/Vector.h>
#include <casacore/casa/OS/Timer.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>

#include <lsqr_solver/GlobalTypedefs.h>
#include <lsqr_solver/SparseMatrix.h>
#include <lsqr_solver/LSQRSolver.h>
#include <lsqr_solver/ModelDamping.h>

#include <askap/AskapLogging.h>
ASKAP_LOGGER(logger, ".linearsolver");

#include <iostream>

#include <string>
#include <map>

#include <cmath>
using std::abs;
using std::map;
using std::string;

namespace askap
{
  namespace scimath
  {
    BOOST_CONSTEXPR_OR_CONST double LinearSolver::KeepAllSingularValues;

    /// @brief Constructor
    /// @details Optionally, it is possible to limit the condition number of
    /// normal equation matrix to a given number.
    /// @param maxCondNumber maximum allowed condition number of the range
    /// of the normal equation matrix for the SVD algorithm. Effectively this
    /// puts the limit on the singular values, which are considered to be
    /// non-zero (all greater than the largest singular value divided by this
    /// condition number threshold). Default is 1e3. Put a negative number
    /// if you don't want to drop any singular values (may be a not very wise
    /// thing to do!). A very large threshold has the same effect. Zero
    /// threshold is not allowed and will cause an exception.
    LinearSolver::LinearSolver(double maxCondNumber) :
           itsMaxCondNumber(maxCondNumber),
           itsWorkersComm(NULL),
           itsMajorLoopIterationNumber(0)
    {
      ASKAPASSERT(itsMaxCondNumber!=0);
    };

    void LinearSolver::init()
    {
      resetNormalEquations();
    }

/// @brief test that all matrix elements are below tolerance by absolute value
/// @details This is a helper method to test all matrix elements
/// @param[in] matr matrix to test
/// @param[in] tolerance tolerance on the element absolute values
/// @return true if all elements are zero within the tolerance
bool LinearSolver::allMatrixElementsAreZeros(const casa::Matrix<double> &matr, const double tolerance)
{
  for (casa::uInt row = 0; row < matr.nrow(); ++row) {
       for (casa::uInt col = 0; col < matr.ncolumn(); ++col) {
            if (abs(matr(row,col)) > tolerance) {
                return false;
            }
       }
  }
  return true;
}

/// @brief extract an independent subset of parameters
/// @details This method analyses the normal equations and forms a subset of
/// parameters which can be solved for independently. Although the SVD is more than
/// capable of dealing with degeneracies, it is often too slow if the number of parameters is large.
/// This method essentially gives the solver a hint based on the structure of the equations
/// @param[in] names names for parameters to choose from
/// @param[in] tolerance tolerance on the matrix elements to decide whether they can be considered independent
/// @return names of parameters in this subset
std::vector<std::string> LinearSolver::getIndependentSubset(std::vector<std::string> &names, const double tolerance) const
{
    ASKAPTRACE("LinearSolver::getIndependentSubset");
    ASKAPDEBUGASSERT(names.size() > 0);
    std::vector<std::string> resultNames;
    resultNames.reserve(names.size());
    resultNames.push_back(names[0]);

    // this has been added to the subset, so delete it
    names.erase(names.begin());

    // for each name in subset (which grows as matches are found), check all remaining names for associates.
    for (std::vector<std::string>::const_iterator ciRes = resultNames.begin(); ciRes != resultNames.end(); ++ciRes) {

        // keep a temporary list of added names to erase from the main list
        std::vector<std::string> toErase;
        toErase.reserve(names.size());

        for (std::vector<std::string>::const_iterator ci = names.begin(); ci != names.end(); ++ci) {
            const casa::Matrix<double>& nm1 = normalEquations().normalMatrix(*ci, *ciRes);
            const casa::Matrix<double>& nm2 = normalEquations().normalMatrix(*ciRes, *ci);

            if (!allMatrixElementsAreZeros(nm1,tolerance) || !allMatrixElementsAreZeros(nm2,tolerance)) {
                // this parameter (iterated in the inner loop) belongs to the subset
                resultNames.push_back(*ci);
                // also add it to the temporary list of added names to erased
                toErase.push_back(*ci);
            }
        }

        // erase all of the names that have just been added from the main list
        for (std::vector<std::string>::iterator it0 = toErase.begin(); it0 != toErase.end(); ++it0) {
            std::vector<std::string>::iterator it = std::find(names.begin(), names.end(), *it0);
            if (it != names.end()) names.erase(it);
        }

    }
    return resultNames;
}

/// @brief solve for a subset of parameters
/// @details This method is used in solveNormalEquations
/// @param[in] params parameters to be updated
/// @param[in] quality Quality of the solution
/// @param[in] names names of the parameters to solve for
std::pair<double,double> LinearSolver::solveSubsetOfNormalEquations(Params &params, Quality& quality,
                   const std::vector<std::string> &names) const
{
    ASKAPTRACE("LinearSolver::solveSubsetOfNormalEquations");
    std::pair<double,double> result(0.,0.);

    bool algorithmLSQR = (algorithm() == "LSQR");

    // Solving A^T Q^-1 V = (A^T Q^-1 A) P

    int nParameters = 0;

    std::vector<std::pair<string, int> > indices(names.size());
    std::map<string, size_t> indicesMap;

    {
        std::vector<std::pair<string, int> >::iterator it = indices.begin();
        for (vector<string>::const_iterator cit=names.begin(); cit!=names.end(); ++cit,++it) {
            ASKAPDEBUGASSERT(it != indices.end());
            it->second = nParameters;
            it->first = *cit;

            indicesMap[it->first] = (size_t)(it->second);

            ASKAPLOG_DEBUG_STR(logger, "Processing " << *cit << " " << nParameters);
            const casa::uInt newParameters = normalEquations().dataVector(*cit).nelements();
            nParameters += newParameters;
            ASKAPDEBUGASSERT((params.isFree(*cit) ? params.value(*cit).nelements() : newParameters) == newParameters);
        }
    }
    ASKAPLOG_DEBUG_STR(logger, "Done");
    ASKAPCHECK(nParameters > 0, "No free parameters in a subset of normal equations");

    ASKAPDEBUGASSERT(indices.size() > 0);

    gsl_matrix * A = 0;
    gsl_vector * B = 0;
    gsl_vector * X = 0;

    if (!algorithmLSQR) {
        // Containers to convert the normal equations to gsl format.
        A = gsl_matrix_alloc (nParameters, nParameters);
        B = gsl_vector_alloc (nParameters);
        X = gsl_vector_alloc (nParameters);
    }

    const GenericNormalEquations& gne = dynamic_cast<const GenericNormalEquations&>(normalEquations());
    size_t nElements = gne.getNumberElements();

    ASKAPLOG_INFO_STR(logger, "Linear solver nParameters = " << nParameters << ", nElements = " << nElements);

    //------------------------------------------------------------------------------
    // Define LSQR solver sparse matrix.
    //------------------------------------------------------------------------------
    void* comm = NULL;
    size_t nrows = 0;
    size_t nnz = 0;
    bool addSmoothnessConstraints = false;

    if (algorithmLSQR) {

#ifdef HAVE_MPI
        MPI_Comm comm_world = MPI_COMM_WORLD;
        comm = (void *)&comm_world;
#endif

        // Define approximate number of nonzero elements.
        // Note: nElements may contain elements which are not currently being solved for.
        if (nElements <= nParameters * nParameters) {
            nnz = nElements;
        } else {
            nnz = nParameters * nParameters;
        }

        nrows = nParameters;
        if (parameters().count("smoothing") > 0
            && parameters().at("smoothing") == "true") {
            addSmoothnessConstraints = true;
        }

        if (addSmoothnessConstraints) {
            nrows += nParameters;
            nnz += 2 * nParameters;
        }
    }
    lsqr::SparseMatrix matrix(nrows, nnz, comm);

    //--------------------------------------------------------------------------------------------
    // Copy matrix elements from normal matrix (map of map of matrixes) to the solver matrix:
    //      - to gsl NxN matrix, for the SVD solver;
    //      - to sparse matrix (CSR format), for the LSQR solver.
    //--------------------------------------------------------------------------------------------

    // Loop over matrix rows.
    for (std::vector<std::pair<string, int> >::const_iterator indit1 = indices.begin();
            indit1 != indices.end(); ++indit1) {

        const std::map<string, casa::Matrix<double> >::const_iterator colItBeg = gne.getNormalMatrixRowBegin(indit1->first);
        const std::map<string, casa::Matrix<double> >::const_iterator colItEnd = gne.getNormalMatrixRowEnd(indit1->first);

        ASKAPCHECK(colItBeg != colItEnd, "Normal matrix has no elements for row = " << indit1->first << ", this shouldn't happen!");

        const casa::uInt nrow = colItBeg->second.nrow();

        for (size_t row = 0; row < nrow; ++row) {

            if (algorithmLSQR) {
                matrix.NewRow();
            }

            // Loop over column elements.
            for (std::map<string, casa::Matrix<double> >::const_iterator colIt = colItBeg;
                    colIt != colItEnd; ++colIt) {

                const std::map<string, size_t>::const_iterator indicesMapIt = indicesMap.find(colIt->first);
                if (indicesMapIt != indicesMap.end()) {
                // It is a parameter to solve for, adding it to the matrix.

                    const size_t colIndex = indicesMapIt->second;
                    const casa::Matrix<double>& nm = colIt->second;

                    ASKAPCHECK(nrow == nm.nrow(), "Not consistent normal matrix element element dimension!");

                    const size_t ncolumn = nm.ncolumn();
                    for (size_t col = 0; col < ncolumn; ++col) {
                         const double elem = nm(row, col);
                         ASKAPCHECK(!std::isnan(elem), "Normal matrix seems to have NaN for row = "<< row << " and col = " << col << ", this shouldn't happen!");

                         if (algorithmLSQR) {
                             matrix.Add(elem, col + colIndex);

                         } else {
                             gsl_matrix_set(A, row + (indit1->second), col + colIndex, elem);
                         }
                    }
                }
            }
        }
    }

    if (algorithmLSQR) {
        size_t nonzeros = matrix.GetNumberElements();
        ASKAPLOG_INFO_STR(logger, "Linear solver Jacobian nonzeros = " << nonzeros);
        ASKAPLOG_INFO_STR(logger, "Linear solver Jacobian sparsity = " << (double)(nonzeros) / (double)(nParameters * nParameters));
    }

    if (!algorithmLSQR) {
        for (std::vector<std::pair<string, int> >::const_iterator indit1=indices.begin();indit1!=indices.end(); ++indit1) {
            const casa::Vector<double> &dv = normalEquations().dataVector(indit1->first);
            for (size_t row=0; row<dv.nelements(); ++row) {
                 const double elem = dv(row);
                 ASKAPCHECK(!std::isnan(elem), "Data vector seems to have NaN for row = "<<row<<", this shouldn't happen!");
                 gsl_vector_set(B, row+(indit1->second), elem);
            }
        }
    }

      /*
      // temporary code to export matrices, which cause problems with the GSL
      // to write up a clear case
      {
        std::ofstream os("dbg.dat");
        os<<nParameters<<std::endl;
        for (int row=0;row<nParameters;++row) {
             for (int col=0;col<nParameters;++col) {
                  if (col) {
                      os<<" ";
                  }
                  os<<gsl_matrix_get(A,row,col);
             }
             os<<std::endl;
        }
      }
      // end of the temporary code
      */

    if (algorithm() == "SVD") {
        ASKAPLOG_INFO_STR(logger, "Solving normal equations using the SVD solver");

        gsl_matrix * V = gsl_matrix_alloc (nParameters, nParameters);
        ASKAPDEBUGASSERT(V!=NULL);
        gsl_vector * S = gsl_vector_alloc (nParameters);
        ASKAPDEBUGASSERT(S!=NULL);
        gsl_vector * work = gsl_vector_alloc (nParameters);
        ASKAPDEBUGASSERT(work!=NULL);

        gsl_error_handler_t *oldhandler=gsl_set_error_handler_off();
        ASKAPLOG_DEBUG_STR(logger, "Running SV decomp");
        const int status = gsl_linalg_SV_decomp (A, V, S, work);
        // ASKAPCHECK(status == 0, "gsl_linalg_SV_decomp failed, status = "<<status);
        gsl_set_error_handler(oldhandler);

        // a hack for now. For some reason, for some matrices gsl_linalg_SV_decomp may return NaN as singular value, perhaps some
        // numerical precision issue inside SVD. Although it needs to be investigated further  (see ASKAPSDP-2270), for now trying
        // to replace those singular values with zeros to exclude them from processing. Note, singular vectors may also contain NaNs
        for (int i=0; i<nParameters; ++i) {
          if (std::isnan(gsl_vector_get(S,i))) {
              gsl_vector_set(S,i,0.);
          }
          for (int k=0; k < nParameters; ++k) {
               ASKAPCHECK(!std::isnan(gsl_matrix_get(V,i,k)), "NaN in V: i="<<i<<" k="<<k);
          }
        }

         // end of the hack

         //SVDecomp (A, V, S);

         // code to put a limit on the condition number of the system
         const double singularValueLimit = nParameters>1 ?
                     gsl_vector_get(S,0)/itsMaxCondNumber : -1.;
         for (int i=1; i<nParameters; ++i) {
              if (gsl_vector_get(S,i)<singularValueLimit) {
                  gsl_vector_set(S,i,0.);
              }
         }

        /*
        // temporary code for debugging
        {
          std::ofstream os("dbg2.dat");
          for (int i=0; i<nParameters; ++i) {
               os<<i<<" "<<gsl_vector_get(S,i)<<std::endl;
          }

          //std::cout<<"new singular value spectrum is ready"<<std::endl;
          //char tst;
          //std::cin>>tst;

        }
        // end of temporary code
        */

         gsl_vector * X = gsl_vector_alloc(nParameters);
         ASKAPDEBUGASSERT(X!=NULL);

         const int solveStatus = gsl_linalg_SV_solve (A, V, S, B, X);
         ASKAPCHECK(solveStatus == 0, "gsl_linalg_SV_solve failed");

// Now find the statistics for the decomposition
         int rank=0;
         double smin = 1e50;
         double smax = 0.0;
         for (int i=0;i<nParameters; ++i) {
              const double sValue = std::abs(gsl_vector_get(S, i));
              ASKAPCHECK(!std::isnan(sValue), "Got NaN as a singular value for normal matrix, this shouldn't happen S[i]="<<gsl_vector_get(S,i)<<" parameter "<<i<<" singularValueLimit="<<singularValueLimit);
              if(sValue>0.0) {
                 ++rank;
                 if ((sValue>smax) || (i == 0)) {
                     smax=sValue;
                 }
                 if ((sValue<smin) || (i == 0)) {
                     smin=sValue;
                 }
               }
         }
         result.first = smin;
         result.second = smax;

         quality.setDOF(nParameters);
         if (status != 0) {
             ASKAPLOG_WARN_STR(logger, "Solution is considered invalid due to gsl_linalg_SV_decomp failure, main matrix is effectively rank zero");
             quality.setRank(0);
             quality.setCond(0.);
             quality.setInfo("SVD decomposition rank deficient");
         } else {
             quality.setRank(rank);
             quality.setCond(smax/smin);
             if (rank==nParameters) {
                 quality.setInfo("SVD decomposition rank complete");
             } else {
                 quality.setInfo("SVD decomposition rank deficient");
             }
         }

// Update the parameters for the calculated changes. Exploit reference
// semantics of casa::Array.
         std::vector<std::pair<string, int> >::const_iterator indit;
         for (indit=indices.begin();indit!=indices.end();++indit) {
              casa::IPosition vecShape(1, params.value(indit->first).nelements());
              casa::Vector<double> value(params.value(indit->first).reform(vecShape));
              for (size_t i=0; i<value.nelements(); ++i)  {
//                 std::cout << value(i) << " " << gsl_vector_get(X, indit->second+i) << std::endl;
                   const double adjustment = gsl_vector_get(X, indit->second+i);
                   ASKAPCHECK(!std::isnan(adjustment), "Solution resulted in NaN as an update for parameter "<<(indit->second + i));
                   value(i) += adjustment;
              }
          }
          gsl_vector_free(S);
          gsl_vector_free(work);
          gsl_matrix_free(V);
    }
    else if (algorithmLSQR) {
        ASKAPLOG_INFO_STR(logger, "Solving normal equations using the LSQR solver");

        int myrank = 0;
        int nbproc = 1;

        lsqr::Vector b_RHS(nrows, 0.);

        // Define the right-hand side (the data misfit part).
        for (std::vector<std::pair<string, int> >::const_iterator indit1=indices.begin();indit1!=indices.end(); ++indit1) {
            const casa::Vector<double> &dv = normalEquations().dataVector(indit1->first);
            for (size_t row=0; row<dv.nelements(); ++row) {
                 const double elem = dv(row);
                 ASKAPCHECK(!std::isnan(elem), "Data vector seems to have NaN for row = "<<row<<", this shouldn't happen!");
                 //gsl_vector_set(B, row+(indit1->second), elem);
                 b_RHS[row+(indit1->second)] = elem;
            }
        }

        if (addSmoothnessConstraints) {
        //-----------------------------------------------
        // Adding smoothness constraints.
        //-----------------------------------------------
            // Setting the smoothing weight.
            double smoothingWeight = 0.;
            if (addSmoothnessConstraints) {
                double smoothingMinWeight = 0.;
                if (parameters().count("smoothingMinWeight") > 0) {
                    smoothingMinWeight = std::atof(parameters().at("smoothingMinWeight").c_str());
                }

                double smoothingMaxWeight = 3.e+6;
                if (parameters().count("smoothingMaxWeight") > 0) {
                    smoothingMaxWeight = std::atof(parameters().at("smoothingMaxWeight").c_str());
                }

                size_t nsteps = 10;
                if (parameters().count("smoothingNsteps") > 0) {
                    nsteps = std::atoi(parameters().at("smoothingNsteps").c_str());
                }

                if (itsMajorLoopIterationNumber < nsteps) {
                    if (smoothingMinWeight == smoothingMaxWeight) {
                        smoothingWeight = smoothingMaxWeight;
                    } else {
                        double span = smoothingMaxWeight - smoothingMinWeight;
                        ASKAPCHECK(span > 0, "Wrong smoothing weight!");

                        // Logarithmic sweep (between the min and max weights).
                        smoothingWeight = smoothingMinWeight + std::pow(10., log10(span) / (double)(nsteps) * (double)(itsMajorLoopIterationNumber));
                    }
                } else {
                    // Relaxation with constant weight.
                    smoothingWeight = smoothingMaxWeight;
                }
            }
            ASKAPLOG_INFO_STR(logger, "Adding smoothness constraints, with weight = " << smoothingWeight);

            // Reading the number of channels.
            size_t nChannels = 0;
            if (parameters().count("nChan") > 0) {
                nChannels = std::atoi(parameters().at("nChan").c_str());
            }
            ASKAPCHECK(nChannels > 1, "Wrong number of channels for smoothness constraints!");

            // Extract the solution at the current major iteration (before the update).
            std::vector<double> x0(nParameters);
            int counter = 0;
            for (std::vector<std::pair<string, int> >::const_iterator indit = indices.begin();
                             indit != indices.end(); ++indit) {
                casa::IPosition vecShape(1, params.value(indit->first).nelements());
                casa::Vector<double> value(params.value(indit->first).reform(vecShape));
                for (size_t i=0; i<value.nelements(); ++i) {
                    x0[indit->second + i] = value(i);
                    counter++;
                }
            }
            ASKAPCHECK(counter == nParameters, "Wrong number of parameters!");

            // Build indexes maps (needed to add smoothness constraints to the matrix).
            std::map<std::pair<casa::uInt, std::string>, size_t> gainIndexesReal;
            std::map<std::pair<casa::uInt, std::string>, size_t> gainIndexesImag;
            for (std::vector<std::pair<string, int> >::const_iterator indit = indices.begin();
                 indit != indices.end(); ++indit) {
                // Make sure there are two unknowns per parameter: real and imaginary parts of the complex gain value.
                ASKAPCHECK(params.value(indit->first).nelements() == 2, "Number of unknowns per parameter name is not correct!");

                // Extracting channel and parameter name.
                std::pair<casa::uInt, std::string> paramInfo = extractChannelInfo(indit->first);

                gainIndexesReal.insert(make_pair(paramInfo, indit->second));     // real part
                gainIndexesImag.insert(make_pair(paramInfo, indit->second + 1)); // imaginary part
            }

            double cost = 0.;
            for (std::vector<std::pair<string, int> >::const_iterator indit = indices.begin();
                             indit != indices.end(); ++indit) {
                // Extracting channel and parameter name.
                std::pair<casa::uInt, std::string> paramInfo = extractChannelInfo(indit->first);
                size_t channel = paramInfo.first;

                bool lastChannel = (channel == nChannels - 1);
                if (!lastChannel) {

                    std::string name = paramInfo.second;

                    size_t currIndex[2];
                    size_t nextIndex[2];

                    // Extracting parameter indexes for the current channel.
                    currIndex[0] = gainIndexesReal[make_pair(channel, name)];
                    currIndex[1] = gainIndexesImag[make_pair(channel, name)];

                    ASKAPCHECK((int)currIndex[0] == indit->second, "Wrong index (real)!");
                    ASKAPCHECK((int)currIndex[1] == indit->second + 1, "Wrong index (imag)!");

                    // Extracting parameter indexes for the next channel.
                    nextIndex[0] = gainIndexesReal[make_pair(channel + 1, name)];
                    nextIndex[1] = gainIndexesImag[make_pair(channel + 1, name)];

                    for (size_t i = 0; i < 2; ++i) {
                        matrix.NewRow();

                        // Applying forward difference grad(f) = f[i+1] - f[i].
                        matrix.Add(- smoothingWeight, currIndex[i]);
                        matrix.Add(+ smoothingWeight, nextIndex[i]);

                        double b_RHS_value = - smoothingWeight * (x0[nextIndex[i]] - x0[currIndex[i]]);

                        size_t b_index = matrix.GetCurrentNumberRows() - 1;
                        b_RHS[b_index] = b_RHS_value;

                        cost += b_RHS_value * b_RHS_value;
                    }
                } else
                {   // No constraints explicitly added for the last channel (it is coupled with previous one by forward difference).
                    // Two rows: for real & imaginary parts.
                    matrix.NewRow();
                    matrix.NewRow();
                }
            }
            ASKAPLOG_INFO_STR(logger, "Smoothness constraints cost (weighted) = " << cost);
            ASKAPLOG_INFO_STR(logger, "Smoothness constraints cost = " << cost / (smoothingWeight * smoothingWeight));
        }
        size_t ncolumms = nParameters;

        // Completed the matrix building.
        matrix.Finalize(ncolumms);

        // A simple approximation for the upper bound of the rank of the  A'A matrix.
        size_t rank_approx = matrix.GetNumberNonemptyRows();

        //-----------------------------------------------
        // Adding damping.
        //-----------------------------------------------
        // Setting damping parameters.
        double alpha = 0.01;
        if (parameters().count("alpha") > 0) {
            alpha = std::atof(parameters().at("alpha").c_str());
        }

        double norm = 2.0;
        if (parameters().count("norm") > 0) {
            norm = std::atof(parameters().at("norm").c_str());
        }

        ASKAPLOG_INFO_STR(logger, "Adding model damping, with alpha = " << alpha);

        lsqr::ModelDamping damping(ncolumms);
        damping.Add(alpha, norm, matrix, b_RHS, NULL, NULL, NULL, myrank, nbproc);

        //-----------------------------------------------
        // Calculating the total cost.
        //-----------------------------------------------
        double total_cost = 0.;
        for (size_t i = 0; i < b_RHS.size(); ++i) {
            total_cost += b_RHS[i] * b_RHS[i];
        }
        ASKAPLOG_INFO_STR(logger, "Total cost = " << total_cost);

        //-----------------------------------------------
        // Setting solver parameters.
        //-----------------------------------------------
        int niter = 100;
        if (parameters().count("niter") > 0) {
            niter = std::atoi(parameters().at("niter").c_str());
        }

        double rmin = 1.e-13;
        if (parameters().count("rmin") > 0) {
            rmin = std::atof(parameters().at("rmin").c_str());
        }

        bool suppress_output = true;
        if (parameters().count("verbose") > 0
            && parameters().at("verbose") == "true") {
            suppress_output = false;
        }

        //-----------------------------------------------
        // Solving the matrix system.
        //-----------------------------------------------
        casa::Timer timer;
        timer.mark();

        lsqr::Vector x(ncolumms, 0.0);
        lsqr::LSQRSolver solver(matrix.GetCurrentNumberRows(), ncolumms);

        solver.Solve(niter, rmin, matrix, b_RHS, x, myrank, nbproc, suppress_output);

        ASKAPLOG_INFO_STR(logger, "Completed LSQR in " << timer.real() << " seconds");

        //------------------------------------------------------------------------
        // Update the parameters for the calculated changes.
        // Exploit reference semantics of casa::Array.
        //------------------------------------------------------------------------
        std::vector<std::pair<string, int> >::const_iterator indit;
        for (indit=indices.begin();indit!=indices.end();++indit) {
            casa::IPosition vecShape(1, params.value(indit->first).nelements());
            casa::Vector<double> value(params.value(indit->first).reform(vecShape));
            for (size_t i=0; i<value.nelements(); ++i) {
                const double adjustment = x[indit->second + i];
                ASKAPCHECK(!std::isnan(adjustment), "Solution resulted in NaN as an update for parameter "<<(indit->second + i));
                value(i) += adjustment;
            }
        }

         //------------------------------------------------------------------------
         // Set approximate solution quality.
         quality.setDOF(nParameters);
         quality.setRank(rank_approx);
    }
    else if (algorithm() == "Chol") {
        // TODO: It seems this branch is never actually used. Need to remove it?
        ASKAPLOG_INFO_STR(logger, "Solving normal equations using the Cholesky decomposition solver");

        quality.setInfo("Cholesky decomposition");
        gsl_linalg_cholesky_decomp(A);
        gsl_linalg_cholesky_solve(A, B, X);
        // Update the parameters for the calculated changes.
        std::vector<std::pair<string, int> >::const_iterator indit;
        for (indit=indices.begin();indit!=indices.end();++indit)
        {
          casa::IPosition vecShape(1, params.value(indit->first).nelements());
          casa::Vector<double> value(params.value(indit->first).reform(vecShape));
          for (size_t i=0; i<value.nelements(); ++i)  {
               value(i)+=gsl_vector_get(X, indit->second+i);
          }
        }
    }
    else {
        ASKAPTHROW(AskapError, "Unknown calibration solver type: " << algorithm());
    }

    if (!algorithmLSQR) {
        // Free up gsl storage.
        gsl_matrix_free(A);
        gsl_vector_free(B);
        gsl_vector_free(X);
    }

    return result;
}

    /// @brief solve for parameters
    /// The solution is constructed from the normal equations and given
    /// parameters are updated. If there are no free parameters in the
    /// given Params class, all unknowns in the normal
    /// equatons will be solved for.
    /// @param[in] params parameters to be updated
    /// @param[in] quality Quality of solution
    /// @note This is fully general solver for the normal equations for any shape
    /// parameters.
    bool LinearSolver::solveNormalEquations(Params &params, Quality& quality)
    {
        ASKAPTRACE("LinearSolver::solveNormalEquations");

        // Solving A^T Q^-1 V = (A^T Q^-1 A) P
        // Find all the free parameters.
        vector<string> names(params.freeNames());
        if (names.size() == 0) {
            // List of parameters is empty, will solve for all
            // unknowns in the equation.
            names = normalEquations().unknowns();
        }
        ASKAPCHECK(names.size() > 0, "No free parameters in Linear Solver");

        if (names.size() < 100 // No need to extract independent blocks if number of unknowns is small.
            || algorithm() == "LSQR") {
            solveSubsetOfNormalEquations(params, quality, names);
        } else {
            while (names.size() > 0) {
                const std::vector<std::string> subsetNames = getIndependentSubset(names,1e-6);
                solveSubsetOfNormalEquations(params, quality, subsetNames);
            }
        }
        return true;
    };

    Solver::ShPtr LinearSolver::clone() const
    {
      return Solver::ShPtr(new LinearSolver(*this));
    }

    void LinearSolver::SetWorkersCommunicator(void *comm)
    {
        itsWorkersComm = comm;
    }

    void LinearSolver::SetMajorLoopIterationNumber(size_t it)
    {
        itsMajorLoopIterationNumber = it;
    }

    // NOTE: Copied from "calibaccess/CalParamNameHelper.h", as currently accessors depends of scimath.
    /// @brief extract coded channel and parameter name
    /// @details This is a reverse operation to codeInChannel. Note, no checks are done that the name passed
    /// has coded channel present.
    /// @param[in] name full name of the parameter
    /// @return a pair with extracted channel and the base parameter name
    std::pair<casa::uInt, std::string> LinearSolver::extractChannelInfo(const std::string &name)
    {
      size_t pos = name.rfind(".");
      ASKAPCHECK(pos != std::string::npos, "Expect dot in the parameter name passed to extractChannelInfo, name="<<name);
      ASKAPCHECK(pos + 1 != name.size(), "Parameter name="<<name<<" ends with a dot");
      return std::pair<casa::uInt, std::string>(utility::fromString<casa::uInt>(name.substr(pos+1)),name.substr(0,pos));
    }

  }
}


// Copyright (c) 2008-2017 Emil Dotchevski and Reverge Studios, Inc.

// 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)

#ifndef BOOST_QVM_2C807EC599D5E980B2EAC9CC53BF67D6
#define BOOST_QVM_2C807EC599D5E980B2EAC9CC53BF67D6

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/deduce_scalar.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/error.hpp>
#include <boost/qvm/gen/vec_assign3.hpp>
#include <boost/qvm/math.hpp>
#include <boost/qvm/static_assert.hpp>
#include <boost/qvm/throw_exception.hpp>

namespace boost {
namespace qvm {
template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<vec_traits<A>::dim == 3 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator+(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      vec_traits<A>::template read_element<0>(a) +
      vec_traits<B>::template read_element<0>(b);
  vec_traits<R>::template write_element<1>(r) =
      vec_traits<A>::template read_element<1>(a) +
      vec_traits<B>::template read_element<1>(b);
  vec_traits<R>::template write_element<2>(r) =
      vec_traits<A>::template read_element<2>(a) +
      vec_traits<B>::template read_element<2>(b);
  return r;
}

namespace sfinae {
using ::boost::qvm::operator+;
}

namespace qvm_detail {
template <int D> struct plus_vv_defined;

template <> struct plus_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<vec_traits<A>::dim == 3 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator-(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      vec_traits<A>::template read_element<0>(a) -
      vec_traits<B>::template read_element<0>(b);
  vec_traits<R>::template write_element<1>(r) =
      vec_traits<A>::template read_element<1>(a) -
      vec_traits<B>::template read_element<1>(b);
  vec_traits<R>::template write_element<2>(r) =
      vec_traits<A>::template read_element<2>(a) -
      vec_traits<B>::template read_element<2>(b);
  return r;
}

namespace sfinae {
using ::boost::qvm::operator-;
}

namespace qvm_detail {
template <int D> struct minus_vv_defined;

template <> struct minus_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3 && vec_traits<B>::dim == 3,
                         A &>::type
    operator+=(A &a, B const &b) {
  vec_traits<A>::template write_element<0>(a) +=
      vec_traits<B>::template read_element<0>(b);
  vec_traits<A>::template write_element<1>(a) +=
      vec_traits<B>::template read_element<1>(b);
  vec_traits<A>::template write_element<2>(a) +=
      vec_traits<B>::template read_element<2>(b);
  return a;
}

namespace sfinae {
using ::boost::qvm::operator+=;
}

namespace qvm_detail {
template <int D> struct plus_eq_vv_defined;

template <> struct plus_eq_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3 && vec_traits<B>::dim == 3,
                         A &>::type
    operator-=(A &a, B const &b) {
  vec_traits<A>::template write_element<0>(a) -=
      vec_traits<B>::template read_element<0>(b);
  vec_traits<A>::template write_element<1>(a) -=
      vec_traits<B>::template read_element<1>(b);
  vec_traits<A>::template write_element<2>(a) -=
      vec_traits<B>::template read_element<2>(b);
  return a;
}

namespace sfinae {
using ::boost::qvm::operator-=;
}

namespace qvm_detail {
template <int D> struct minus_eq_vv_defined;

template <> struct minus_eq_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<vec_traits<A>::dim == 3 && is_scalar<B>::value,
                              deduce_vec<A>>::type
    operator*(A const &a, B b) {
  typedef typename deduce_vec<A>::type R;
  R r;
  vec_traits<R>::template write_element<0>(r) =
      vec_traits<A>::template read_element<0>(a) * b;
  vec_traits<R>::template write_element<1>(r) =
      vec_traits<A>::template read_element<1>(a) * b;
  vec_traits<R>::template write_element<2>(r) =
      vec_traits<A>::template read_element<2>(a) * b;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int D> struct mul_vs_defined;

template <> struct mul_vs_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_scalar<A>::value && vec_traits<B>::dim == 3,
                              deduce_vec<B>>::type
    operator*(A a, B const &b) {
  typedef typename deduce_vec<B>::type R;
  R r;
  vec_traits<R>::template write_element<0>(r) =
      a * vec_traits<B>::template read_element<0>(b);
  vec_traits<R>::template write_element<1>(r) =
      a * vec_traits<B>::template read_element<1>(b);
  vec_traits<R>::template write_element<2>(r) =
      a * vec_traits<B>::template read_element<2>(b);
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int D> struct mul_sv_defined;

template <> struct mul_sv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3 && is_scalar<B>::value,
                         A &>::type
    operator*=(A &a, B b) {
  vec_traits<A>::template write_element<0>(a) *= b;
  vec_traits<A>::template write_element<1>(a) *= b;
  vec_traits<A>::template write_element<2>(a) *= b;
  return a;
}

namespace sfinae {
using ::boost::qvm::operator*=;
}

namespace qvm_detail {
template <int D> struct mul_eq_vs_defined;

template <> struct mul_eq_vs_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<vec_traits<A>::dim == 3 && is_scalar<B>::value,
                              deduce_vec<A>>::type
    operator/(A const &a, B b) {
  typedef typename deduce_vec<A>::type R;
  R r;
  vec_traits<R>::template write_element<0>(r) =
      vec_traits<A>::template read_element<0>(a) / b;
  vec_traits<R>::template write_element<1>(r) =
      vec_traits<A>::template read_element<1>(a) / b;
  vec_traits<R>::template write_element<2>(r) =
      vec_traits<A>::template read_element<2>(a) / b;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator/;
}

namespace qvm_detail {
template <int D> struct div_vs_defined;

template <> struct div_vs_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3 && is_scalar<B>::value,
                         A &>::type
    operator/=(A &a, B b) {
  vec_traits<A>::template write_element<0>(a) /= b;
  vec_traits<A>::template write_element<1>(a) /= b;
  vec_traits<A>::template write_element<2>(a) /= b;
  return a;
}

namespace sfinae {
using ::boost::qvm::operator/=;
}

namespace qvm_detail {
template <int D> struct div_eq_vs_defined;

template <> struct div_eq_vs_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class R, class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_vec<A>::value && vec_traits<R>::dim == 3 &&
                             vec_traits<A>::dim == 3,
                         R>::type
    convert_to(A const &a) {
  R r;
  vec_traits<R>::template write_element<0>(r) =
      vec_traits<A>::template read_element<0>(a);
  vec_traits<R>::template write_element<1>(r) =
      vec_traits<A>::template read_element<1>(a);
  vec_traits<R>::template write_element<2>(r) =
      vec_traits<A>::template read_element<2>(a);
  return r;
}

namespace sfinae {
using ::boost::qvm::convert_to;
}

namespace qvm_detail {
template <int D> struct convert_to_v_defined;

template <> struct convert_to_v_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3 && vec_traits<B>::dim == 3,
                         bool>::type
    operator==(A const &a, B const &b) {
  return vec_traits<A>::template read_element<0>(a) ==
             vec_traits<B>::template read_element<0>(b) &&
         vec_traits<A>::template read_element<1>(a) ==
             vec_traits<B>::template read_element<1>(b) &&
         vec_traits<A>::template read_element<2>(a) ==
             vec_traits<B>::template read_element<2>(b);
}

namespace sfinae {
using ::boost::qvm::operator==;
}

namespace qvm_detail {
template <int D> struct eq_vv_defined;

template <> struct eq_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3 && vec_traits<B>::dim == 3,
                         bool>::type
    operator!=(A const &a, B const &b) {
  return !(vec_traits<A>::template read_element<0>(a) ==
           vec_traits<B>::template read_element<0>(b)) ||
         !(vec_traits<A>::template read_element<1>(a) ==
           vec_traits<B>::template read_element<1>(b)) ||
         !(vec_traits<A>::template read_element<2>(a) ==
           vec_traits<B>::template read_element<2>(b));
}

namespace sfinae {
using ::boost::qvm::operator!=;
}

namespace qvm_detail {
template <int D> struct neq_vv_defined;

template <> struct neq_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<vec_traits<A>::dim == 3, deduce_vec<A>>::type
    operator-(A const &a) {
  typedef typename deduce_vec<A>::type R;
  R r;
  vec_traits<R>::template write_element<0>(r) =
      -vec_traits<A>::template read_element<0>(a);
  vec_traits<R>::template write_element<1>(r) =
      -vec_traits<A>::template read_element<1>(a);
  vec_traits<R>::template write_element<2>(r) =
      -vec_traits<A>::template read_element<2>(a);
  return r;
}

namespace sfinae {
using ::boost::qvm::operator-;
}

namespace qvm_detail {
template <int D> struct minus_v_defined;

template <> struct minus_v_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_vec<A>::value && vec_traits<A>::dim == 3,
                         typename vec_traits<A>::scalar_type>::type
    mag(A const &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T const a0 = vec_traits<A>::template read_element<0>(a);
  T const a1 = vec_traits<A>::template read_element<1>(a);
  T const a2 = vec_traits<A>::template read_element<2>(a);
  T const m2 = a0 * a0 + a1 * a1 + a2 * a2;
  T const mag = sqrt<T>(m2);
  return mag;
}

namespace sfinae {
using ::boost::qvm::mag;
}

namespace qvm_detail {
template <int D> struct mag_v_defined;

template <> struct mag_v_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<is_vec<A>::value && vec_traits<A>::dim == 3,
                         typename vec_traits<A>::scalar_type>::type
    mag_sqr(A const &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T const a0 = vec_traits<A>::template read_element<0>(a);
  T const a1 = vec_traits<A>::template read_element<1>(a);
  T const a2 = vec_traits<A>::template read_element<2>(a);
  T const m2 = a0 * a0 + a1 * a1 + a2 * a2;
  return m2;
}

namespace sfinae {
using ::boost::qvm::mag_sqr;
}

namespace qvm_detail {
template <int D> struct mag_sqr_v_defined;

template <> struct mag_sqr_v_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<vec_traits<A>::dim == 3, deduce_vec<A>>::type
    normalized(A const &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T const a0 = vec_traits<A>::template read_element<0>(a);
  T const a1 = vec_traits<A>::template read_element<1>(a);
  T const a2 = vec_traits<A>::template read_element<2>(a);
  T const m2 = a0 * a0 + a1 * a1 + a2 * a2;
  if (m2 == scalar_traits<typename vec_traits<A>::scalar_type>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  T const rm = scalar_traits<T>::value(1) / sqrt<T>(m2);
  typedef typename deduce_vec<A>::type R;
  R r;
  vec_traits<R>::template write_element<0>(r) = a0 * rm;
  vec_traits<R>::template write_element<1>(r) = a1 * rm;
  vec_traits<R>::template write_element<2>(r) = a2 * rm;
  return r;
}

namespace sfinae {
using ::boost::qvm::normalized;
}

template <class A>
BOOST_QVM_INLINE_OPERATIONS
    typename enable_if_c<vec_traits<A>::dim == 3, void>::type
    normalize(A &a) {
  typedef typename vec_traits<A>::scalar_type T;
  T const a0 = vec_traits<A>::template read_element<0>(a);
  T const a1 = vec_traits<A>::template read_element<1>(a);
  T const a2 = vec_traits<A>::template read_element<2>(a);
  T const m2 = a0 * a0 + a1 * a1 + a2 * a2;
  if (m2 == scalar_traits<typename vec_traits<A>::scalar_type>::value(0))
    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
  T const rm = scalar_traits<T>::value(1) / sqrt<T>(m2);
  vec_traits<A>::template write_element<0>(a) *= rm;
  vec_traits<A>::template write_element<1>(a) *= rm;
  vec_traits<A>::template write_element<2>(a) *= rm;
}

namespace sfinae {
using ::boost::qvm::normalize;
}

namespace qvm_detail {
template <int D> struct normalize_v_defined;

template <> struct normalize_v_defined<3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    vec_traits<A>::dim == 3 && vec_traits<B>::dim == 3,
    deduce_scalar<typename vec_traits<A>::scalar_type,
                  typename vec_traits<B>::scalar_type>>::type
dot(A const &a, B const &b) {
  typedef typename vec_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  typedef typename deduce_scalar<Ta, Tb>::type Tr;
  Ta const a0 = vec_traits<A>::template read_element<0>(a);
  Ta const a1 = vec_traits<A>::template read_element<1>(a);
  Ta const a2 = vec_traits<A>::template read_element<2>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  Tr const dot = a0 * b0 + a1 * b1 + a2 * b2;
  return dot;
}

namespace sfinae {
using ::boost::qvm::dot;
}

namespace qvm_detail {
template <int D> struct dot_vv_defined;

template <> struct dot_vv_defined<3> { static bool const value = true; };
} // namespace qvm_detail

} // namespace qvm
} // namespace boost

#endif


#ifndef SETTINGS_COMMON_HPP
#define SETTINGS_COMMON_HPP

#include <mpi.h>
#include <boost/lexical_cast.hpp>
#include <clstatphys/physics/toda_lax_form.hpp>
#include <clstatphys/lattice/chain.hpp>

using Lattice = lattice::Chain;

struct SettingsCommon{
  int num_iterations = 32;
  int Ns = 10; // lenght of chain
  int N_loop = 1; //loop number
  int Ns_observe = Ns; //observed particle
  int N_mc = 10; //numer of time step
  int order = 0;
  int num_particles = 10; //nummer of particles
  int N_mpi_parallel = 2; // number of mpi parallel
  int N_each = 10;// number of each parallel
  int N_adj = 2;// number of adjacent spins
  int N_total_data = 10; // number of data
  int process_id = 1;
  int name_length;
  int num_threads;
  int mpi_error = 1;
  int mpi_error_end = 1;
  char processor_name[MPI_MAX_PROCESSOR_NAME];
  std::string condition_dat{"condi.dat"};
  std::string result_directory{"./"};

  SettingsCommon(int argc, char **argv, int & input_counter){
    if(mpi_error > 0){
      mpi_error = MPI_Init(&argc, &argv);
      MPI_Comm_size(MPI_COMM_WORLD, &N_mpi_parallel);
      MPI_Comm_rank(MPI_COMM_WORLD, &process_id);
      MPI_Get_processor_name(processor_name, &name_length);
    }
    set(argc, argv, input_counter);
  }
  SettingsCommon() = default;

  inline void set(int argc, char **argv, int & input_counter){
    if (argc > input_counter) result_directory = boost::lexical_cast<std::string>(argv[input_counter]); ++input_counter;
    if (argc > input_counter) Ns =               boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) Ns_observe =       boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (Ns < Ns_observe){
      std::cerr << "Ns_observe should be lower than Ns" << std::endl;
      std::exit(1);
    }
    if (argc > input_counter) N_mc =             boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) N_loop =           boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) num_iterations =   boost::lexical_cast<int>(argv[input_counter]);++input_counter;

    num_particles = Lattice::set_num_particles(Ns); //nummer of particles
    Lattice lattice_t(Ns);
    N_adj = lattice_t.number_adjacent();

    N_each = N_loop / N_mpi_parallel; // number of each parallel
    condition_dat = result_directory + condition_dat;

    N_total_data = N_mc;
  }

  template <class Dataput>
  inline void declare(Dataput & dataput){
    dataput << "<<Common Settings>>" << std::endl
            << "  Sampling GGE of Toda lattice by Monte Carlo" << std::endl
            <<  "  " << Lattice::name() << std::endl

            << "  Number of patricles : num_particles = " << num_particles << std::endl
            << "  Length of whole system : Ns = " << Ns << std::endl
            << "  Length of observe system : Ns_observe = " << Ns_observe << std::endl
            << "  Number of result data (time step): N_total_data = " << N_total_data << std::endl
            << "  Number of monte carlo steps : N_mc = " << N_mc << std::endl
            << "  Number of MPI parallelization : N_mpi_parallel =" << N_mpi_parallel << std::endl
            << "  Number of loop : N_loop =" << N_loop << std::endl
            << "  Each thread loop : N_each = " << N_each << std::endl
            << "  Number of iteration for action variables : num_iterations = " << num_iterations << std::endl
            << "  Result directory : " << result_directory << std::endl;
  }

  integrable::TodaLaxForm toda_lax_form(){
    Lattice lattice_t(Ns);
    std::vector<std::vector<int> > pair_table(
                                              num_particles,
                                              std::vector<int>(N_adj)
                                              );
    lattice_t.create_table(pair_table);
    return integrable::TodaLaxForm(num_particles,1.0,1.0,"periodic");
  }

  inline void finalize(){mpi_error = MPI_Finalize();};

}; // end struct define

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file VectorValues.cpp
 * @brief Implementations for VectorValues
 * @author Richard Roberts
 * @author Alex Cunningham
 */

#include <gtsam/linear/VectorValues.h>

#include <boost/bind.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/map.hpp>

using namespace std;

namespace gtsam {

  using boost::combine;
  using boost::adaptors::transformed;
  using boost::adaptors::map_values;
  using boost::accumulate;

  /* ************************************************************************* */
  VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
  {
    // Merge using predicate for comparing first of pair
    merge(first.begin(), first.end(), second.begin(), second.end(), inserter(values_, values_.end()),
      boost::bind(&less<Key>::operator(), less<Key>(), boost::bind(&KeyValuePair::first, _1), boost::bind(&KeyValuePair::first, _2)));
    if(size() != first.size() + second.size())
      throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
  }

  /* ************************************************************************* */
  VectorValues::VectorValues(const Vector& x, const Dims& dims) {
    typedef pair<Key, size_t> Pair;
    size_t j = 0;
    for (const Pair& v : dims) {
      Key key;
      size_t n;
      boost::tie(key, n) = v;
      values_.emplace(key, x.segment(j, n));
      j += n;
    }
  }

  /* ************************************************************************* */
  VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
    size_t j = 0;
    for (const SlotEntry& v : scatter) {
      values_.emplace(v.key, x.segment(j, v.dimension));
      j += v.dimension;
    }
  }

  /* ************************************************************************* */
  VectorValues VectorValues::Zero(const VectorValues& other)
  {
    VectorValues result;
    for(const KeyValuePair& v: other)
      result.values_.emplace(v.first, Vector::Zero(v.second.size()));
    return result;
  }

  /* ************************************************************************* */
  VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
    std::pair<iterator, bool> result = values_.insert(key_value);
    if(!result.second)
      throw std::invalid_argument(
      "Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
      + "' already in this VectorValues.");
    return result.first;
  }

  /* ************************************************************************* */
  VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
    std::pair<iterator, bool> result = values_.emplace(j, value);
    if(!result.second)
      throw std::invalid_argument(
      "Requested to emplace variable '" + DefaultKeyFormatter(j)
      + "' already in this VectorValues.");
    return result.first;
  }

  /* ************************************************************************* */
  void VectorValues::update(const VectorValues& values)
  {
    iterator hint = begin();
    for(const KeyValuePair& key_value: values)
    {
      // Use this trick to find the value using a hint, since we are inserting from another sorted map
      size_t oldSize = values_.size();
      hint = values_.insert(hint, key_value);
      if(values_.size() > oldSize) {
        values_.unsafe_erase(hint);
        throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
      } else {
        hint->second = key_value.second;
      }
    }
  }

  /* ************************************************************************* */
  void VectorValues::insert(const VectorValues& values)
  {
    size_t originalSize = size();
    values_.insert(values.begin(), values.end());
    if(size() != originalSize + values.size())
      throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
  }

  /* ************************************************************************* */
  void VectorValues::setZero()
  {
    for(Vector& v: values_ | map_values)
      v.setZero();
  }

  /* ************************************************************************* */
  ostream& operator<<(ostream& os, const VectorValues& v) {
    // Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB
    map<Key, Vector> sorted;
    for (const auto& key_value : v) {
      sorted.emplace(key_value.first, key_value.second);
    }
    for (const auto& key_value : sorted)
#else
    for (const auto& key_value : v)
#endif
    {
      os << "  " << StreamedKey(key_value.first) << ": " << key_value.second.transpose()
         << "\n";
    }
    return os;
  }

  /* ************************************************************************* */
  void VectorValues::print(const string& str,
                           const KeyFormatter& formatter) const {
    cout << str << ": " << size() << " elements\n";
    cout << key_formatter(formatter) << *this;
    cout.flush();
}

  /* ************************************************************************* */
  bool VectorValues::equals(const VectorValues& x, double tol) const {
    if(this->size() != x.size())
      return false;
    for(const auto& values: boost::combine(*this, x)) {
      if(values.get<0>().first != values.get<1>().first ||
        !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
        return false;
    }
    return true;
  }

  /* ************************************************************************* */
  Vector VectorValues::vector() const {
    // Count dimensions
    DenseIndex totalDim = 0;
    for (const Vector& v : *this | map_values) totalDim += v.size();

    // Copy vectors
    Vector result(totalDim);
    DenseIndex pos = 0;
    for (const Vector& v : *this | map_values) {
      result.segment(pos, v.size()) = v;
      pos += v.size();
    }

    return result;
  }

  /* ************************************************************************* */
  Vector VectorValues::vector(const Dims& keys) const
  {
    // Count dimensions
    DenseIndex totalDim = 0;
    for(size_t dim: keys | map_values)
      totalDim += dim;
    Vector result(totalDim);
    size_t j = 0;
    for(const Dims::value_type& it: keys) {
      result.segment(j,it.second) = at(it.first);
      j += it.second;
    }
    return result;
  }

  /* ************************************************************************* */
  void VectorValues::swap(VectorValues& other) {
    this->values_.swap(other.values_);
  }

  /* ************************************************************************* */
  namespace internal
  {
    bool structureCompareOp(const boost::tuple<VectorValues::value_type,
      VectorValues::value_type>& vv)
    {
      return vv.get<0>().first == vv.get<1>().first
        && vv.get<0>().second.size() == vv.get<1>().second.size();
    }
  }

  /* ************************************************************************* */
  bool VectorValues::hasSameStructure(const VectorValues other) const
  {
    return accumulate(combine(*this, other)
      | transformed(internal::structureCompareOp), true, logical_and<bool>());
  }

  /* ************************************************************************* */
  double VectorValues::dot(const VectorValues& v) const
  {
    if(this->size() != v.size())
      throw invalid_argument("VectorValues::dot called with a VectorValues of different structure");
    double result = 0.0;
    typedef boost::tuple<value_type, value_type> ValuePair;
    using boost::adaptors::map_values;
    for(const ValuePair& values: boost::combine(*this, v)) {
      assert_throw(values.get<0>().first == values.get<1>().first,
        invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
      assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
        invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
      result += values.get<0>().second.dot(values.get<1>().second);
    }
    return result;
  }

  /* ************************************************************************* */
  double VectorValues::norm() const {
    return std::sqrt(this->squaredNorm());
  }

  /* ************************************************************************* */
  double VectorValues::squaredNorm() const {
    double sumSquares = 0.0;
    using boost::adaptors::map_values;
    for(const Vector& v: *this | map_values)
      sumSquares += v.squaredNorm();
    return sumSquares;
  }

  /* ************************************************************************* */
  VectorValues VectorValues::operator+(const VectorValues& c) const
  {
    if(this->size() != c.size())
      throw invalid_argument("VectorValues::operator+ called with different vector sizes");
    assert_throw(hasSameStructure(c),
      invalid_argument("VectorValues::operator+ called with different vector sizes"));

    VectorValues result;
    // The result.end() hint here should result in constant-time inserts
    for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
      result.values_.emplace(j1->first, j1->second + j2->second);

    return result;
  }

  /* ************************************************************************* */
  VectorValues VectorValues::add(const VectorValues& c) const
  {
    return *this + c;
  }

  /* ************************************************************************* */
  VectorValues& VectorValues::operator+=(const VectorValues& c)
  {
    if(this->size() != c.size())
      throw invalid_argument("VectorValues::operator+= called with different vector sizes");
    assert_throw(hasSameStructure(c),
      invalid_argument("VectorValues::operator+= called with different vector sizes"));

    iterator j1 = begin();
    const_iterator j2 = c.begin();
    // The result.end() hint here should result in constant-time inserts
    for(; j1 != end(); ++j1, ++j2)
      j1->second += j2->second;

    return *this;
  }

  /* ************************************************************************* */
  VectorValues& VectorValues::addInPlace(const VectorValues& c)
  {
    return *this += c;
  }

  /* ************************************************************************* */
  VectorValues& VectorValues::addInPlace_(const VectorValues& c)
  {
    for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
      pair<VectorValues::iterator, bool> xi = tryInsert(j2->first, Vector());
      if(xi.second)
        xi.first->second = j2->second;
      else
        xi.first->second += j2->second;
    }
    return *this;
  }

  /* ************************************************************************* */
  VectorValues VectorValues::operator-(const VectorValues& c) const
  {
    if(this->size() != c.size())
      throw invalid_argument("VectorValues::operator- called with different vector sizes");
    assert_throw(hasSameStructure(c),
      invalid_argument("VectorValues::operator- called with different vector sizes"));

    VectorValues result;
    // The result.end() hint here should result in constant-time inserts
    for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
      result.values_.emplace(j1->first, j1->second - j2->second);

    return result;
  }

  /* ************************************************************************* */
  VectorValues VectorValues::subtract(const VectorValues& c) const
  {
    return *this - c;
  }

  /* ************************************************************************* */
  VectorValues operator*(const double a, const VectorValues &v)
  {
    VectorValues result;
    for(const VectorValues::KeyValuePair& key_v: v)
      result.values_.emplace(key_v.first, a * key_v.second);
    return result;
  }

  /* ************************************************************************* */
  VectorValues VectorValues::scale(const double a) const
  {
    return a * *this;
  }

  /* ************************************************************************* */
  VectorValues& VectorValues::operator*=(double alpha)
  {
    for(Vector& v: *this | map_values)
      v *= alpha;
    return *this;
  }

  /* ************************************************************************* */
  VectorValues& VectorValues::scaleInPlace(double alpha)
  {
    return *this *= alpha;
  }

  /* ************************************************************************* */

} // \namespace gtsam


#include <boost/algorithm/string.hpp>
#include <iostream>
using namespace std;

const string alphabet = "ABCDEFGHIJKLMNOPQRSTUVWXYZ";
const int alphabet_length = (int) alphabet.length();

string get_keyphrase(const string& text, const string& keyword)
{
  /* Returns the Vigenere keyword of appropriate length (in all caps) for a
   * given text (either plaintext or ciphertext) and keyword.
   */
  string keyphrase = "";
  string caps_keyword = boost::algorithm::to_upper_copy<string>(keyword);

  size_t keyword_length = keyword.length();
  size_t text_length = text.length();

  for (int i = 0; i < text_length; i++)
  {
    keyphrase += caps_keyword[i % keyword_length];
  }

  return keyphrase;
}

string encrypt(const string& plaintext, const string& keyword)
{
  /* Returns the ciphertext produced by encrypting the plaintext using the
   * given keyword and the Vigenere Cipher
   * (http://en.wikipedia.org/wiki/Vigen%C3%A8re_cipher).
   */

  string ciphertext = "";
  string caps_plaintext = boost::algorithm::to_upper_copy<string>(plaintext);
  string keyphrase = get_keyphrase(plaintext, keyword);

  for (int i = 0; i < plaintext.length(); i++)
  {
    int plaintext_index = (int) alphabet.find(caps_plaintext[i]);
    int advance_by = (int) alphabet.find(keyphrase[i]);
    int ciphertext_index = (plaintext_index + advance_by) % alphabet_length;

    ciphertext += alphabet[ciphertext_index];
  }

  return ciphertext;
}

string decrypt(const string& ciphertext, const string& keyword)
{
  /* Returns the plaintext produced by decrypting the ciphertext using the
   * given keyword and the Vigenere Cipher
   * (http://en.wikipedia.org/wiki/Vigen%C3%A8re_cipher).
   */
  string plaintext = "";
  string caps_ciphertext = boost::algorithm::to_upper_copy<string>(ciphertext);
  string keyphrase = get_keyphrase(ciphertext, keyword);

  for (int i = 0; i < ciphertext.length(); i++)
  {
    int ciphertext_index = (int) alphabet.find(caps_ciphertext[i]);
    int reverse_by = (int) alphabet.find(keyphrase[i]);
    int plaintext_index = (ciphertext_index - reverse_by) % alphabet_length;

    cout << ciphertext_index << endl;
    cout << reverse_by << endl;
    cout << plaintext_index << endl;
    cout << endl;

    plaintext += alphabet[plaintext_index];
  }

  return plaintext;
}