/*
For more information, please see: http://software.sci.utah.edu

The MIT License

Copyright (c) 2012 Scientific Computing and Imaging Institute,
University of Utah.

Permission is hereby granted, free of charge, to any person obtaining a
copy of this software and associated documentation files (the "Software"),
to deal in the Software without restriction, including without limitation
the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included
in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
*/

//    File       : SolveInverseProblemWithTikhonov.cc
//    Author     : Moritz Dannhauer, Ayla Khan, Dan White
//    Date       : November 02th, 2012 (last update)

#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>

#include <Modules/Legacy/Inverse/SolveInverseProblemWithTikhonovImpl.h>

#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>

#include <Core/Logging/LoggerInterface.h>
#include <Core/Utils/Exception.h>

namespace BioPSE
{

using namespace SCIRun;
using namespace SCIRun::Core;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Logging;

TikhonovAlgorithmImpl::TikhonovAlgorithmImpl(const DenseMatrixHandle& forwardMatrix,
                                             const DenseMatrixHandle& measuredData,
                                             AlgorithmChoice regularizationChoice,
                                             AlgorithmSolutionSubcase regularizationSolutionSubcase,
                                             AlgorithmResidualSubcase regularizationResidualSubcase,
                                             const DenseMatrixHandle sourceWeighting,
                                             const DenseMatrixHandle sensorWeighting,
                                             bool computeRegularizedInverse,
                                             LegacyLoggerInterface* pr)
: forwardMatrix_(forwardMatrix),
measuredData_(measuredData),
sourceWeighting_(sourceWeighting),
sensorWeighting_(sensorWeighting),
regularizationChoice_(regularizationChoice),
regularizationSolutionSubcase_(regularizationSolutionSubcase),
regularizationResidualSubcase_(regularizationResidualSubcase),
lambda_(0),
computeRegularizedInverse_(computeRegularizedInverse),
pr_(pr)
{
  //TODO: size checking here.
}

MatrixHandle TikhonovAlgorithmImpl::get_inverse_solution() const
{
  return inverseSolution_;
}

MatrixHandle TikhonovAlgorithmImpl::get_inverse_matrix() const
{
  return inverseMatrix_;
}

DenseColumnMatrixHandle TikhonovAlgorithmImpl::get_regularization_parameter() const
{
  return regularizationParameter_;
}

//! Find Corner, find the maximal curvature which corresponds to the L-curve corner
double
TikhonovAlgorithmImpl::FindCorner(const TikhonovAlgorithm::LCurveInput& input, int& lambda_index)
{
  const std::vector<double>& rho = input.rho_;
  const std::vector<double>& eta = input.eta_;
  const std::vector<double>& lambdaArray = input.lambdaArray_;
  int nLambda = input.nLambda_;

  std::vector<double> deta(nLambda);
  std::vector<double> ddeta(nLambda);
  std::vector<double> drho(nLambda);
  std::vector<double> ddrho(nLambda);
  std::vector<double> lrho(nLambda);
  std::vector<double> leta(nLambda);
  DenseColumnMatrix kapa(nLambda);

  double maxKapa = -1.0e10;
  for (int i = 0; i < nLambda; i++)
  {
    lrho[i] = std::log10(rho[i]);
    leta[i] = std::log10(eta[i]);
    if(i>0)
    {
      deta[i] = (leta[i]-leta[i-1]) / (lambdaArray[i]-lambdaArray[i-1]); // compute first derivative
      drho[i] = (lrho[i]-lrho[i-1]) / (lambdaArray[i]-lambdaArray[i-1]);
    }
    if(i>1)
    {
      ddeta[i] = (deta[i]-deta[i-1]) / (lambdaArray[i]-lambdaArray[i-1]); // compute second derivative from first
      ddrho[i] = (drho[i]-drho[i-1]) / (lambdaArray[i]-lambdaArray[i-1]);
    }
  }
  drho[0] = drho[1];
  deta[0] = deta[1];
  ddrho[0] = ddrho[2];
  ddrho[1] = ddrho[2];
  ddeta[0] = ddeta[2];
  ddeta[1] = ddeta[2];

  lambda_index = 0;
  for (int i = 0; i < nLambda; i++)
  {
    kapa[i] = std::abs((drho[i] * ddeta[i] - ddrho[i] * deta[i]) /  //compute curvature
                       std::sqrt(std::pow((deta[i]*deta[i]+drho[i]*drho[i]), 3.0)));
    if (kapa[i] > maxKapa) // find max curvature
    {
      maxKapa = kapa[i];
      lambda_index = i;
    }
  }

  return lambdaArray[lambda_index];
}

double
TikhonovAlgorithmImpl::LambdaLookup(const TikhonovAlgorithm::LCurveInput& input, double lambda, int& lambda_index, const double epsilon)
{
  const std::vector<double>& lambdaArray = input.lambdaArray_;
  int nLambda = input.nLambda_;

  for (int i = 0; i < nLambda-1; ++i)
  {
    if (i > 0 && (lambda < lambdaArray[i-1] || lambda > lambdaArray[i+1])) continue;

    double lambda_step_midpoint = std::abs(lambdaArray[i+1] - lambdaArray[i])/2;

    if (std::abs(lambda - lambdaArray[i]) <= epsilon)  // TODO: is this a reasonable comparison???
    {
      lambda_index = i;
      return lambdaArray[lambda_index];
    }

    if (std::abs(lambda - lambdaArray[i]) < lambda_step_midpoint)
    {
      lambda_index = i;
      return lambdaArray[lambda_index];
    }

    if (std::abs(lambda - lambdaArray[i+1]) < lambda_step_midpoint)
    {
      lambda_index = i+1;
      return lambdaArray[lambda_index];
    }
  }
  return -1;
}

namespace LinearAlgebra
{
  void solve_lapack(const DenseMatrix& A, const DenseColumnMatrix& b, DenseColumnMatrix& x)
  {
    x = A.lu().solve(b).eval();
  }

  class LapackError : public std::exception {};
}

void TikhonovAlgorithmImpl::run(const TikhonovAlgorithmImpl::Input& input)
{
  // TODO: use DimensionMismatch exception where appropriate
  // DIMENSION CHECK!!
  const int M = forwardMatrix_->nrows();
  const int N = forwardMatrix_->ncols();
  if (M != measuredData_->nrows())
  {
    BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Input matrix dimensions must agree."));
  }
  if (1 != measuredData_->ncols())
  {
    BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Measured data must be a vector"));
  }
  //decide used Tikhonov regularization formulation (underdetermined or overdetermined)
  //based purely on relationship of number of sensors compared to number of source reconstruction points
  //UNDERDETERMINED CASE
  if ( ((M < N) && (regularizationChoice_ == automatic)) || (regularizationChoice_ == underdetermined))
  {
    //.........................................................................
    // OPERATE ON DATA:
    // Compute X = R * R^T * A^T (A * R * R^T * A^T + LAMBDA * LAMBDA * C * C^T) * Y
    //.........................................................................
    DenseMatrix ARRtrAtr, RRtrAtr;
    double lambda=0, lambda_sq=0;
    DenseMatrix forward_transpose = forwardMatrix_->transpose();
    DenseMatrix regMat;
    #ifdef SCIRUN4_CODE_TO_BE_CONVERTED_LATER
    SparseRowMatrixHandle sourceWeighting_sparse;
    #endif
    DenseColumnMatrix solution(M);
    DenseColumnMatrix RRtrAtrsolution(N);

    const DenseMatrixHandle measuredDataRef = measuredData_;
    if (!sourceWeighting_) //check if a Source Space Weighting Matrix exist?
    {
      regMat = DenseMatrix::Identity(N, N);
      ARRtrAtr = *forwardMatrix_ * forward_transpose;
      RRtrAtr = forward_transpose;
    }
    else
    {
      if(((regularizationSolutionSubcase_==solution_constrained) && ((N != sourceWeighting_->nrows()) || (N != sourceWeighting_->ncols()))) ||
         ((regularizationSolutionSubcase_==solution_constrained_squared) && (N != sourceWeighting_->nrows())))
      {
        BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Solution Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of columns) !"));
      }
      regMat = *sourceWeighting_;
      #ifdef SCIRUN4_CODE_TO_BE_CONVERTED_LATER
      sourceWeighting_sparse = sourceWeighting_->sparse();
      #endif

      if (regularizationSolutionSubcase_== solution_constrained)
      {
        RRtrAtr = *sourceWeighting_ * forward_transpose;
	      auto AR = forwardMatrix_;
	      ARRtrAtr = *AR * RRtrAtr;
      }
      else if (regularizationSolutionSubcase_== solution_constrained_squared)
      {
        auto AR = *forwardMatrix_ * *sourceWeighting_;
	      auto RtrAtr = AR.transpose().eval();
        RRtrAtr = *sourceWeighting_ * RtrAtr;
	      ARRtrAtr = AR * RtrAtr;
      }
    }
    DenseMatrix CCtr;
    if (!sensorWeighting_) //check if a Sensor Space (Noise Covariance) Weighting Matrix exist?
    {
      CCtr = DenseMatrix::Identity(M, M);
    }
    else
    {
      if (((regularizationResidualSubcase_ == residual_constrained) && ((M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()))) ||
          ((regularizationResidualSubcase_ == residual_constrained_squared) && (M != sensorWeighting_->nrows())))
      {
        BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Data Residual Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of rows) !"));
      }
      else
      {
        if (regularizationResidualSubcase_ == residual_constrained)
          CCtr = *sensorWeighting_;
        else
          if ( regularizationResidualSubcase_ == residual_constrained_squared )
            CCtr = *sensorWeighting_ * sensorWeighting_->transpose();
      }
    }

    DenseMatrix regForMatrix(M, M);

    //Get Regularization parameter(s) : Lambda
    if ((input.regMethod_ == "single") || (input.regMethod_ == "slider"))
    {
      if (input.regMethod_ == "single")
      {
        // Use single fixed lambda value, entered in UI
        lambda = input.lambdaFromTextEntry_;
      }
      else if (input.regMethod_ == "slider")
      {
        // Use single fixed lambda value, select via slider
        lambda = input.lambdaSlider_;
      }
    }
    else if (input.regMethod_ == "lcurve")
    {
      const int nLambda = input.lambdaCount_;  //declare needed variables

      std::vector<double> lambdaArray(nLambda, 0.0);
      std::vector<double> rho(nLambda, 0.0);
      std::vector<double> eta(nLambda, 0.0);

      lambdaArray[0] = input.lambdaMin_;
      const double lam_step = pow(10.0, log10(input.lambdaMax_ / input.lambdaMin_) / (nLambda-1));

      DenseColumnMatrix Ax(M);
      DenseColumnMatrix Rx(N);

      int lambda_index = 0;
      const DenseMatrix &forwardMatrixRef = *forwardMatrix_;

      int nr_rows = CCtr.nrows();
      int nr_cols = CCtr.ncols();
      const int NR_BOUNDS_CHECK = nr_rows * nr_cols;

      double* AtrA = ARRtrAtr.data();
      double* LLtr = CCtr.data();

      for (int j = 0; j < nLambda; j++)
      {
        if (j)
        {
          lambdaArray[j] = lambdaArray[j-1] * lam_step;
        }

        lambda_sq = lambdaArray[j] * lambdaArray[j]; //generate current lambda

        double* rm = regForMatrix.data();

        for (int i = 0; i < NR_BOUNDS_CHECK; i++)
        {
          rm[i] = AtrA[i] + lambda_sq * LLtr[i];
        }

        try
        {
          LinearAlgebra::solve_lapack(regForMatrix, *measuredDataRef, solution);
        }
        catch (LinearAlgebra::LapackError&)
        {
          const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda Range ''From'' value for the L-Curve calculation.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }
        catch(DimensionMismatch&)
        {
          const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }

        RRtrAtrsolution = RRtrAtr * solution;

        if (sourceWeighting_)
        {
          if (RRtrAtrsolution.nrows() == sourceWeighting_->ncols())
            Rx = *sourceWeighting_ * RRtrAtrsolution;
          else if  (RRtrAtrsolution.nrows() == sourceWeighting_->nrows())
            Rx = sourceWeighting_->transpose() * RRtrAtrsolution;
          else
          {
            const std::string errorMessage(" Solution weighting matrix unexpectedly does not fit to compute the weighted solution norm. ");
            if (pr_)
            {
              pr_->error(errorMessage);
            }
            else
            {
              std::cerr << errorMessage << std::endl;
            }
          }
        }
        else
          Rx = RRtrAtrsolution;

        // Calculate the norm of Ax-b and Rx for L curve
        Ax = forwardMatrixRef * RRtrAtrsolution;

        rho[j]=0; eta[j]=0;
        for (int k = 0; k < Ax.nrows(); k++)
        {
          double T = Ax(k) - (*measuredDataRef)(k);
          rho[j] += T*T; //norm of the data fit term
        }

        for (int k = 0; k < Rx.nrows(); k++)
        {
          double T = Rx[k];
          eta[j] += T*T; //norm of the model term
        }
        // eta and rho needed to plot the Lcurve and determine the L corner
        rho[j] = sqrt(rho[j]);
        eta[j] = sqrt(eta[j]);
      }
      boost::shared_ptr<TikhonovAlgorithm::LCurveInput> lcurveInput(new TikhonovAlgorithm::LCurveInput(rho, eta, lambdaArray, nLambda));
      lcurveInput_handle_ = lcurveInput;
      lambda = FindCorner(*lcurveInput_handle_, lambda_index);

      if (input.updateLCurveGui_)
        input.updateLCurveGui_(lambda, *lcurveInput_handle_, lambda_index);
    }
    lambda_sq = lambda * lambda;
    //compute the solution with the selected regularization parameter: lambda
    regularizationParameter_.reset(new DenseColumnMatrix(1));
    (*regularizationParameter_)(0) = lambda;
    int nr_rows = regForMatrix.nrows();
    int nr_cols = regForMatrix.ncols();

    double* AtrA = ARRtrAtr.data();
    double* RtrR = CCtr.data();
    double* rm   = regForMatrix.data();
    for (int i=0; i<(int)(nr_rows*nr_cols); i++)
    {
      rm[i] = AtrA[i] + lambda_sq * RtrR[i];
    }

    if (computeRegularizedInverse_)
    {
      if (sourceWeighting_)
      {
        inverseMatrix_.reset(new DenseMatrix(RRtrAtr * regForMatrix.inverse()));
      }
      else
      {
        inverseMatrix_.reset(new DenseMatrix(forward_transpose * regForMatrix.inverse()));
      }
      inverseSolution_.reset(new DenseMatrix(*inverseMatrix_ * *measuredData_));
    }
    else
    {
      try
      {
        LinearAlgebra::solve_lapack(regForMatrix, *measuredDataRef, solution);
      }
      catch (LinearAlgebra::LapackError&)
      {
        const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda Range ''From'' value for the L-Curve calculation.");
        if (pr_)
        {
          pr_->error(errorMessage);
        }
        else
        {
          std::cerr << errorMessage << std::endl;
        }
        throw;
      }
      catch(DimensionMismatch&)
      {
        const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
        if (pr_)
        {
          pr_->error(errorMessage);
        }
        else
        {
          std::cerr << errorMessage << std::endl;
        }
        throw;
      }

      RRtrAtrsolution = RRtrAtr * solution;
      inverseSolution_.reset(new DenseMatrix(RRtrAtrsolution));
    }

  }
  else
    //OVERDETERMINED CASE,
    //similar procedure as underdetermined case (documentation comments similar, see above)
    if ( ((regularizationChoice_ == automatic) && (M>=N)) || (regularizationChoice_==overdetermined) )
    {
      //.........................................................................
      // OPERATE ON DATA:
      // Computes X = (A^T * C^T * C * A + LAMBDA * LAMBDA * R^T * R) * A^T * C^T * C *Y
      //.........................................................................
      // calculate A^T * A
      DenseMatrix forward_transpose = forwardMatrix_->transpose();
      DenseMatrix AtrA;
      DenseMatrix CCtr;
      DenseMatrix matrixNoiseCov_transpose;

      if (!sensorWeighting_)
      //check if a Sensor Space (Noise Covariance) Weighting Matrix exist?
      {
        // For large problems, allocating this matrix causes the module to hang.
        // The CCtr matrix is only used if sensorWeighting is available.
        //
        // In general, this code really should be rewritten to use sparse matrices (SCIRun 5)
        // or have replaced with more efficient calculations
        // (Eigen may have algorithms we can use).
        //CCtr = DenseMatrix::identity(M);
        AtrA = forward_transpose * *forwardMatrix_;
      }
      else
      {
        if (((regularizationResidualSubcase_ == residual_constrained) && ((M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()))) ||
            ((regularizationResidualSubcase_ == residual_constrained_squared) && (M != sensorWeighting_->ncols())))
        {
          BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Data Residual Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of rows)!"));
        }

        matrixNoiseCov_transpose = sensorWeighting_->transpose();

        // regularizationResidualSubcase_ is either residual_constrained (default) or residual_constrained_squared
        if (regularizationResidualSubcase_ == residual_constrained)
          CCtr = *sensorWeighting_;
        else
          if (regularizationResidualSubcase_ == residual_constrained_squared)
            CCtr = matrixNoiseCov_transpose * *sensorWeighting_;

        AtrA = forward_transpose * CCtr * *forwardMatrix_;
      }
      // calculate R^T * R
      DenseMatrix RtrR;
      DenseMatrix regMat;

      if (!sourceWeighting_)
      {
        regMat = DenseMatrix::Identity(N, N);
        RtrR = DenseMatrix::Identity(N, N);
      }
      else
      {
        if(((regularizationSolutionSubcase_==solution_constrained) && ((N != sourceWeighting_->nrows()) || (N != sourceWeighting_->ncols()))) ||
           ((regularizationSolutionSubcase_==solution_constrained_squared) && (N != sourceWeighting_->ncols())))
        {
          BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Solution Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of columns) ! "));
        }
        regMat = *sourceWeighting_;

        if (regularizationSolutionSubcase_ == solution_constrained_squared)
          RtrR = sourceWeighting_->transpose() * regMat;
        else if (regularizationSolutionSubcase_ == solution_constrained)
          RtrR = regMat;
      }

      double lambda = 0, lambda_sq = 0;
      int lambda_index = 0;
      // calculate A^T * Y
      DenseColumnMatrix AtrY(N);
      DenseColumnMatrix measuredDataRef;
      if (sensorWeighting_)
      {
        measuredDataRef = CCtr * *measuredData_;
      }
      else
      {
        measuredDataRef = *measuredData_;
      }
      AtrY = forwardMatrix_->transpose() * measuredDataRef;
      DenseMatrix regForMatrix(N, N);
      DenseColumnMatrix solution(N);
      DenseColumnMatrix Ax(M);
      DenseColumnMatrix Rx(N);
      if ((input.regMethod_ == "single") || (input.regMethod_ == "slider"))
      {
        if (input.regMethod_ == "single")
        {
          // Use single fixed lambda value, entered in UI
          lambda = input.lambdaFromTextEntry_;
        }
        else if (input.regMethod_ == "slider")
        {
          // Use single fixed lambda value, select via slider
          lambda = input.lambdaSlider_;
        }
      }
      else if (input.regMethod_ == "lcurve")
      {
        // Use L-curve, lambda from corner of the L-curve
        const int nLambda = input.lambdaCount_;

        std::vector<double> lambdaArray(nLambda);
        std::vector<double> rho(nLambda);
        std::vector<double> eta(nLambda);

        lambdaArray[0] = input.lambdaMin_;
        const double lam_step = pow(10.0, log10(input.lambdaMax_ / input.lambdaMin_) / (nLambda-1));

        double* AtrAptr = AtrA.data();
        double* RtrRptr = RtrR.data();
        double* rm   = regForMatrix.data();

        int s = N*N;
        for (int j = 0; j < nLambda; j++)
        {
          if (j)
          {
            lambdaArray[j] = lambdaArray[j-1] * lam_step;
          }

          lambda_sq = lambdaArray[j] * lambdaArray[j];

          ///////////////////////////////////////
          ////Calculating the solution directly
          ///////////////////////////////////////
          for (int i = 0; i < s; i++)
          {
            rm[i] = AtrAptr[i] + lambda_sq * RtrRptr[i];
          }

          // Before, solution will be equal to (A^T * y)
          // After, solution will be equal to x_reg
          try
          {
            LinearAlgebra::solve_lapack(regForMatrix, AtrY, solution);
          }
          catch (LinearAlgebra::LapackError&)
          {
            const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda value for the L-Curve calculation.");
            if (pr_)
            {
              pr_->error(errorMessage);
            }
            else
            {
              std::cerr << errorMessage << std::endl;
            }
            throw;
          }
          catch(DimensionMismatch&)
          {
            const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
            if (pr_)
            {
              pr_->error(errorMessage);
            }
            else
            {
              std::cerr << errorMessage << std::endl;
            }
            throw;
          }

          ////////////////////////////////
          const DenseMatrix &forwardMatrixRef = *forwardMatrix_;

          Ax = forwardMatrixRef * solution;
          Rx = regMat * solution;
          // Calculate the norm of Ax-b and Rx
          rho[j]=0; eta[j]=0;
          for (int k = 0; k < M; k++)
          {
            double T = Ax(k) - (*measuredData_)(k);
            rho[j] += T*T;
          }

          for (int k = 0; k < N; k++)
          {
            double T = Rx(k);
            eta[j] += T*T;
          }

          rho[j] = sqrt(rho[j]);
          eta[j] = sqrt(eta[j]);
        }
        boost::shared_ptr<TikhonovAlgorithm::LCurveInput> lcurveInput(new TikhonovAlgorithm::LCurveInput(rho, eta, lambdaArray, nLambda));
        lcurveInput_handle_ = lcurveInput;
        lambda = FindCorner(*lcurveInput_handle_, lambda_index);

        if (input.updateLCurveGui_)
          input.updateLCurveGui_(lambda, *lcurveInput_handle_, lambda_index);

      } // END  else if (reg_method_.get() == "lcurve")
      lambda_sq = lambda * lambda;

      regularizationParameter_.reset(new DenseColumnMatrix(1));
      (*regularizationParameter_)[0] = lambda;
      int nr_rows = regForMatrix.nrows();
      int nr_cols = regForMatrix.ncols();
      const int NR_BOUNDS_CHECK = nr_rows * nr_cols;
      const double* AtrAptr = AtrA.data();
      const double* RtrRptr = RtrR.data();
      double* rm   = regForMatrix.data();

      for (int i = 0; i < NR_BOUNDS_CHECK; i++)
      {
        rm[i] = AtrAptr[i] + lambda_sq * RtrRptr[i];
      }
      if (computeRegularizedInverse_)
      {
        if (CCtr.empty())
          inverseMatrix_.reset(new DenseMatrix((regForMatrix.inverse() * forward_transpose)));
        else
          inverseMatrix_.reset(new DenseMatrix((regForMatrix.inverse() * forward_transpose) * CCtr));

        inverseSolution_.reset(new DenseMatrix(*inverseMatrix_ * *measuredData_));
      }
      else
      {
        try
        {
          LinearAlgebra::solve_lapack(regForMatrix, AtrY, solution);
        }
        catch (LinearAlgebra::LapackError&)
        {
          const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda value for the L-Curve calculation.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }
        catch(DimensionMismatch&)
        {
          const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }
        inverseSolution_.reset(new DenseMatrix(solution));
      }

    }
}

void TikhonovAlgorithmImpl::update_graph(const TikhonovAlgorithmImpl::Input& input, double lambda, int lambda_index, const double epsilon)
{
  if (lcurveInput_handle_ && input.updateLCurveGui_)
  {
    lambda = LambdaLookup(*lcurveInput_handle_, lambda, lambda_index, epsilon);
    if (lambda >= 0)
    {
      input.updateLCurveGui_(lambda, *lcurveInput_handle_, lambda_index);
    }
  }
}

#if 0

void SolveInverseProblemWithTikhonov::tcl_command(GuiArgs& args, void* userdata)
{
  if (args[1] == "updategraph" && args.count() == 4)
  {
    double lambda = boost::lexical_cast<double>(args[2]);
    int lambda_index = boost::lexical_cast<double>(args[3]);

    if (input_handle_.get() != 0 && algo_handle_.get() != 0)
    {
      algo_handle_->update_graph(*input_handle_, lambda, lambda_index, lambda_resolution_.get());
    }
  }
  else
  {
    // Relay data to the Module class
    Module::tcl_command(args, userdata);
  }
}

#endif

TikhonovAlgorithm::LCurveInput::LCurveInput(const std::vector<double>& rho, const std::vector<double>& eta, const std::vector<double>& lambdaArray, int nLambda)
: rho_(rho), eta_(eta), lambdaArray_(lambdaArray), nLambda_(nLambda)
{}

TikhonovAlgorithmImpl::Input::Input(const std::string& regMethod, double lambdaFromTextEntry, double lambdaSlider, int lambdaCount, double lambdaMin, double lambdaMax,
                                    lcurveGuiUpdate updateLCurveGui)
: regMethod_(regMethod), lambdaFromTextEntry_(lambdaFromTextEntry), lambdaSlider_(lambdaSlider), lambdaCount_(lambdaCount), lambdaMin_(lambdaMin), lambdaMax_(lambdaMax),
updateLCurveGui_(updateLCurveGui)
{}

} // End namespace BioPSE


#pragma once

#include <stdio.h>
#include <stdlib.h>
#include <tchar.h>
#include <math.h>

// STL
#include <algorithm>
#include <functional>
#include <cmath>

#include <list>
#include <map>
#include <unordered_map>
#include <vector>
#include <set>
#include <unordered_set>
#include <iterator>

#include <iostream>
#include <fstream>
#include <sstream>

#include <cerrno>
#include <cassert>

using namespace std;
using namespace std::tr1;

// SFML - http://www.sfml-dev.org/index.php
#include <SFML/Graphics/Drawable.hpp>
#include <SFML/Graphics/Color.hpp>
#include <SFML/Graphics/RenderTarget.hpp>
#include <SFML/Graphics/Text.hpp>
#include <SFML/System/String.hpp>
#include <SFML/System/Time.hpp>
#include <SFML/Audio.hpp>
#include <SFML/Window.hpp>
#include <SFML/Graphics.hpp>
#include <SFML/OpenGL.hpp>

// Box2D - http://box2d.org/
#include <Box2D/Box2D.h>

// Lua
#include <lua/lua.hpp>
#include <lua/lauxlib.h>
#include <lua/lualib.h>

// Boost - http://www.boost.org/
#define BOOST_NO_MEMBER_TEMPLATE_FRIENDS
#include "boost/multi_index/ordered_index.hpp"
#include "boost/multi_index/hashed_index.hpp"
#include <boost/multi_index/identity.hpp>
#include <boost/multi_index/mem_fun.hpp>
#include <boost/multi_index/member.hpp>
#include "boost/multi_index/sequenced_index.hpp"
#include "Boost/tuple/tuple.hpp"
#include "Boost/any.hpp"
#include "Boost/bind.hpp"
#include "Boost/function.hpp"
#include "boost/lexical_cast.hpp"
#include "boost/multi_index_container.hpp"
#include "Boost/ref.hpp"
#include "Boost/signals2.hpp"
using namespace boost;
using namespace boost::multi_index;

// RadpiXML
#include "rapidxml/rapidxml.hpp"
using namespace rapidxml;

// typedefs
typedef unsigned int u32;
typedef unsigned short u16;
typedef unsigned char u8;
typedef int s32;
typedef short s16;
typedef char s8;

// helper macros
#define FOREACH( iterator, collection ) for( auto iterator(collection.begin()); iterator != collection.end(); iterator++ )
#define FOREACH_R( iterator, collection ) for( auto iterator(collection.rbegin()); iterator != collection.rend(); iterator++ )

#define COMBINE2(a, b) a##b
#define COMBINE(a, b) COMBINE2(a,b)

#define STATIC_INITIALISE_START			private: static struct StaticInitialise { StaticInitialise()
#define STATIC_INITIALISE_END			} s_xStaticInitialise;
#define STATIC_INITIALISE_RUN( TParentClass )	TParentClass::StaticInitialise TParentClass::s_xStaticInitialise;

// Core classes
#include "Core/Maths/Maths.hpp"
#include "Core/Data/Hash.hpp"
#include "Core/Data/TUID.hpp"
#include "Core/Data/Data.hpp"


#include "ros_utils.hpp"
#include "kinematics.hpp"
#include <boost/array.hpp>

#include <casadi/casadi.hpp>

#include <gtsam_node/CanData.h>
#include <gtsam_node/ArticulatedAngles.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "ackermann_msgs/AckermannDrive.h"

namespace gtsam_node
{

    void estimateEst(Vec3& qCarEst,const Vec3& qLoc,const Vec3& qPred,const MheParams& estParams)
    {

        qCarEst[0] = estParams.WeightTh*qPred[0] + (1-estParams.WeightTh)*qLoc[0];
        qCarEst[1] = estParams.WeightPos*qPred[1] + (1-estParams.WeightPos)*qLoc[1];
        qCarEst[2] = estParams.WeightPos*qPred[2] + (1-estParams.WeightPos)*qLoc[2];

    }

    void estimateEstTrailer(Vec4& qTrailerEst,const Vec4& qTrailerLoc,const Vec4& qTrailerPred,const MheParams& estParams)
    {
        qTrailerEst[0] = estParams.WeightTrailer1*qTrailerPred[0] + (1-estParams.WeightTrailer1)*qTrailerLoc[0];
        qTrailerEst[1] = estParams.WeightTh*qTrailerPred[1] + (1-estParams.WeightTh)*qTrailerLoc[1];
        qTrailerEst[2] = estParams.WeightPos*qTrailerPred[2] + (1-estParams.WeightPos)*qTrailerLoc[2];
        qTrailerEst[3] = estParams.WeightPos*qTrailerPred[3] + (1-estParams.WeightPos)*qTrailerLoc[3];
    }
}


#include "cnpy/cnpy.h"
#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

namespace po = boost::program_options;

void colorizeCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
    // pack r/g/b into rgb
    std::uint8_t r = 0, g = 255, b = 0;
    std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
    for (auto& p : *cloud) {
        p.rgb = *reinterpret_cast<float*>(&rgb);
    }
}

static inline std::string cvTypeToStr(int type)
{
    std::string r;

    uchar depth = type & CV_MAT_DEPTH_MASK;
    uchar chans = 1 + (type >> CV_CN_SHIFT);

    switch (depth) {
        case CV_8U:
            r = "8U";
            break;
        case CV_8S:
            r = "8S";
            break;
        case CV_16U:
            r = "16U";
            break;
        case CV_16S:
            r = "16S";
            break;
        case CV_32S:
            r = "32S";
            break;
        case CV_32F:
            r = "32F";
            break;
        case CV_64F:
            r = "64F";
            break;
        default:
            r = "User";
            break;
    }

    r += "C";
    r += (chans + '0');

    return r;
}

int main(int argc, char** argv)
{
    // Program options
    po::options_description desc("Read different data and send to python\n\n"
                                 "Options");
    std::string pcdpath, imgpath;
    desc.add_options()("help,h", "Print this help")(
        "pcdpath,p", po::value<std::string>(&pcdpath)->required(), "Set the input path for the .pcd")(
        "imgpath,i", po::value<std::string>(&imgpath)->required(), "Set the input path for the image");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    // --- Processing Normal (Numpy) Matrix

    Eigen::Matrix3d rotMat;
    Eigen::Affine3d htMat{Eigen::Affine3d::Identity()};
    rotMat = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX());
    htMat.translate(Eigen::Vector3d(20, 30, 40));
    htMat.rotate(rotMat);
    auto htMatMatrix = htMat.matrix();
    // we need the following due to Eigen stores data as column-major
    htMatMatrix.transposeInPlace();
    double* htMatData = htMatMatrix.data();
    cnpy::npz_save("out.npz", "ht_matrix", htMatData, {4, 4}, "w");

    std::cout << "htMat: " << std::endl << htMat.matrix() << std::endl;

    // ---

    // Processing Image (OpenCV) Matrix

    auto image = cv::imread(imgpath);
    std::cout << "Image of size: " << image.rows << "x" << image.cols << " of type: " << cvTypeToStr(image.type())
              << std::endl;
    std::vector<uchar> imageArray;
    for (auto i = 0; i < image.rows; i++) {
        for (auto j = 0; j < image.cols; j++) {
            const auto& pixel = image.at<cv::Vec3b>(i, j);
            imageArray.push_back(pixel[0]);
            imageArray.push_back(pixel[1]);
            imageArray.push_back(pixel[2]);
        }
    }
    cnpy::npz_save("out.npz", "img", &imageArray[0], {image.rows, image.cols, 3}, "a");

    cv::imshow("image", image);
    cv::waitKey(0);

    // ---

    // --- Processing PointCloud (PCL) Matrix

    std::cout << "opening pcd: " << pcdpath << std::endl;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(pcdpath, *cloud) == -1) {
        throw std::runtime_error("cannot load pcd");
    }
    colorizeCloud(cloud);

    std::vector<double> pointCloudArray;
    for (const auto& p : *cloud) {
        pointCloudArray.push_back(p.x);
        pointCloudArray.push_back(p.y);
        pointCloudArray.push_back(p.z);
    }
    cnpy::npz_save("out.npz", "pcd_numpy", &pointCloudArray[0], {cloud->size(), 3}, "a");

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {
    }

    // ---

    return 0;
}


//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@gmail.com>
//
// 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_GIL_EXTENSION_HISTOGRAM_STL_HISTOGRAM_HPP
#define BOOST_GIL_EXTENSION_HISTOGRAM_STL_HISTOGRAM_HPP

#include <boost/gil/concepts/concept_check.hpp>
#include <boost/gil/gray.hpp>
#include <boost/gil/histogram.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/image_view_factory.hpp>

#include <array>
#include <map>
#include <utility>
#include <vector>

namespace boost { namespace gil {

//////////////////////////////////////////////////////////
/// Histogram extension for STL container
//////////////////////////////////////////////////////////
/// \defgroup Histogram - STL Containers
/// \brief Collection of functions to provide histogram support in GIL using Standard
///        Template Library Containers
/// The conversion from Boost.GIL images to compatible histograms are provided. The supported
/// container types would be std::vector, std::array, std::map.
///
/// Some general constraints on STL extension:-
/// 1. Supports only 1D histogram.
/// 2. Cannot use signed images with compatible random access containers.
/// 3. Automatic resize of std::array in case of shortage of bins, to ensure
///    correctness comes before performance.
/// 4. Container key type (if exists) has to be one of std::integral types to be
///    GIL compatible.
/// 5. Container value type has to be of std::arithmetic types.
///

///
/// \ingroup Histogram - STL Containers
/// \brief Overload for std::vector of fill_histogram
///
template <typename SrcView, typename T>
void fill_histogram(SrcView const& srcview, std::vector<T>& histogram, bool accumulate = false)
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    static_assert(std::is_arithmetic<T>::value, "Improper container type for images.");
    static_assert(
        std::is_unsigned<typename channel_type<SrcView>::type>::value,
        "Improper container type for signed images.");

    using channel_t = typename channel_type<SrcView>::type;
    using pixel_t   = pixel<channel_t, gray_layout_t>;

    if (!accumulate)
        histogram.clear();
    histogram.resize(std::numeric_limits<channel_t>::max() + 1);

    for_each_pixel(color_converted_view<pixel_t>(srcview), [&](pixel_t const& p) {
        ++histogram[static_cast<std::size_t>(p)];
    });
}

/// \ingroup Histogram - STL Containers
/// \brief Overload for std::array of fill_histogram
///
template <typename SrcView, typename T, std::size_t N>
void fill_histogram(SrcView const& srcview, std::array<T, N>& histogram, bool accumulate = false)
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    static_assert(std::is_arithmetic<T>::value && N > 0, "Improper container type for images.");
    static_assert(
        std::is_unsigned<typename channel_type<SrcView>::type>::value,
        "Improper container type for signed images.");

    using channel_t = typename channel_type<SrcView>::type;
    using pixel_t   = pixel<channel_t, gray_layout_t>;

    const size_t pixel_max = std::numeric_limits<channel_t>::max();
    const float scale      = (histogram.size() - 1.0f) / pixel_max;

    if (!accumulate)
        std::fill(std::begin(histogram), std::end(histogram), 0);

    for_each_pixel(color_converted_view<pixel_t>(srcview), [&](pixel_t const& p) {
        ++histogram[static_cast<std::size_t>(p * scale)];
    });
}

/// \ingroup Histogram - STL Containers
/// \brief Overload for std::map of fill_histogram
///
template <typename SrcView, typename T1, typename T2>
void fill_histogram(SrcView const& srcview, std::map<T1, T2>& histogram, bool accumulate = false)
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    static_assert(
        std::is_arithmetic<T1>::value && std::is_integral<T2>::value,
        "Improper container type for images.");

    using channel_t = typename channel_type<SrcView>::type;
    using pixel_t   = pixel<channel_t, gray_layout_t>;

    if (!accumulate)
        histogram.clear();

    for_each_pixel(color_converted_view<pixel_t>(srcview), [&](pixel_t const& p) {
        ++histogram[static_cast<std::size_t>(p)];
    });
}

/// \ingroup Histogram - STL Containers
/// \brief Overload for std::vector of cumulative_histogram
///
template <typename T>
std::vector<T> cumulative_histogram(std::vector<T>& hist)
{
    std::vector<T> cumulative_hist(hist.size());
    static_assert(std::is_arithmetic<T>::value, "Improper container type for images.");
    T cumulative_counter = 0;
    for (std::size_t i = 0; i < hist.size(); i++)
    {
        cumulative_counter += hist[i];
        cumulative_hist[i] = cumulative_counter;
    }
    return cumulative_hist;
}

/// \ingroup Histogram - STL Containers
/// \brief Overload for std::array of cumulative_histogram
///
template <typename T, std::size_t N>
std::array<T, N> cumulative_histogram(std::array<T, N>& histogram)
{
    std::array<T, N> cumulative_hist;
    static_assert(std::is_arithmetic<T>::value && N > 0, "Improper container type for images.");
    T cumulative_counter = 0;
    for (std::size_t i = 0; i < N; i++)
    {
        cumulative_counter += histogram[i];
        cumulative_hist[i] = cumulative_counter;
    }
    return cumulative_hist;
}

/// \ingroup Histogram - STL Containers
/// \brief Overload for std::map of cumulative_histogram
///
template <typename T1, typename T2>
std::map<T1, T2> cumulative_histogram(std::map<T1, T2>& histogram)
{
    std::map<T1, T2> cumulative_hist;
    static_assert(
        std::is_arithmetic<T1>::value && std::is_integral<T2>::value,
        "Improper container type for images.");
    T2 cumulative_counter = 0;
    for (auto const& it : histogram)
    {
        cumulative_counter += it.second;
        cumulative_hist[it.first] = cumulative_counter;
    }
    return cumulative_hist;
}

}}  // namespace boost::gil

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_NOT_EQUAL_WITH_EQUAL_NANS_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_NOT_EQUAL_WITH_EQUAL_NANS_HPP_INCLUDED
#include <boost/simd/predicates/functions/is_not_equal_with_equal_nans.hpp>
#include <boost/simd/include/functions/simd/is_greater_equal.hpp>
#include <boost/simd/include/functions/simd/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/logical_xor.hpp>
#include <boost/simd/include/functions/simd/logical_not.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( is_not_equal_with_equal_nans_
                                    , tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                      ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return logical_xor(is_greater_equal(a0, a1), is_greater_equal(a1, a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( is_not_equal_with_equal_nans_
                                    , tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                      ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return neq(a0,a1);
    }
  };
} } }

#endif


/**
 * DD cut generation for CPLEX
 */

#ifndef CUT_CPLEX_HPP_
#define CUT_CPLEX_HPP_

#include <ilcplex/ilocplex.h>
#include <vector>
#include <boost/unordered_map.hpp>
#include "../util/graph.hpp"
#include "../util/options.hpp"
#include "../util/util.hpp"
#include "inequality.hpp"
#include "cut_info.hpp"

using namespace std;
using boost::unordered_map;

/** Generate DD cut */
Inequality* generate_bdd_inequality(BDD* bdd, const vector<double>& x, const vector<double>& interior_point, Options* options,
	CutInfo* cut_info=NULL);

// Perturbation methods to increase cut dimension

/** Apply iterative perturbation in order to obtain a facet exactly: optimize in each direction fixing variables */
void perturb_bdd_cut_iterative(IloEnv env, IloCplex cplex, IloModel model, const IloNumVarArray u, IloObjective obj, int nvars,
	const vector<double>& x, const vector<double>& interior_point);

/** Apply random perturbation in order to obtain a facet with high probability */
void perturb_bdd_cut_random(IloEnv env, IloCplex cplex, IloModel model, const IloNumVarArray u, IloObjective obj, int nvars,
	const vector<double>& x, const vector<double>& interior_point);

#endif /* CUT_CPLEX_HPP_ */


#define BOOST_TEST_MODULE "Tests for Gearoenix game engine"
#include <boost/test/included/unit_test.hpp>
// Making types printable for boost
#include <gearoenix/math/gx-math-intersection-status.hpp>

#define GX_TEST_PRINT_TYPE_VAL(x)                                                        \
    namespace boost::test_tools::tt_detail {                                             \
    template <>                                                                          \
    struct print_log_value<gearoenix::x> {                                               \
        void operator()(std::ostream& os, const gearoenix::x v) { ::operator<<(os, v); } \
    };                                                                                \
    }
GX_TEST_PRINT_TYPE_VAL(math::IntersectionStatus)
// Test units
#include "gx-test-ai-dijkstra.hpp"
#include "gx-test-ai-graph.hpp"
#include "gx-test-cr-allocator.hpp"
#include "gx-test-cr-pool.hpp"
#include "gx-test-cr-sync-influence-manager.hpp"
#include "gx-test-math-frustum.hpp"
#include "gx-test-math-vector.hpp"
#include "gx-test-phs-acc-bvh.hpp"
#include "gx-test-rnd-texture.hpp"


/**
 * @file
 * @author  [Yi-Mu "Enoch" Chen](https://github.com/yimuchen)
 * @brief   Implementing measurement ostream interaction.
 */
#ifdef CMSSW_GIT_HASH
#include "UserUtils/Common/interface/Maths.hpp"
#include "UserUtils/Common/interface/STLUtils/StringUtils.hpp"
#include "UserUtils/MathUtils/interface/Measurement.hpp"
#else
#include "UserUtils/Common/Maths.hpp"
#include "UserUtils/Common/STLUtils/StringUtils.hpp"
#include "UserUtils/MathUtils/Measurement.hpp"
#endif

#include <boost/algorithm/string.hpp>
#include <regex>
#include <string>

namespace usr
{

namespace fmt {

/**
 * @brief Re-implementing a double input for common fmt interface
 *
 * This construction means that the measurement without uncertainty would
 * output the same values as if using a double.
 */
decimal::decimal( const double input, const int p ) :
  _central( input ),
  _upper( 0 ),
  _lower( 0 )
{
  precision( p );
}

/**
 * @brief normal construction for typical measurements.
 */
decimal::decimal( const Measurement& input, const int p ) :
  _central( input.CentralValue() ),
  _upper( input.AbsUpperError() ),
  _lower( input.AbsLowerError() )
{
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar status.
 */
decimal::decimal( const RooRealVar& input, const int p ) :
  _central( input.getVal() ),
  _upper( fabs( input.getErrorHi() ) ),
  _lower( fabs( input.getErrorLo() ) )
{
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar status.
 */
decimal::decimal( const RooRealVar* input, const int p ) :
  _central( input->getVal() ),
  _upper( fabs( input->getErrorHi() ) ),
  _lower( fabs( input->getErrorLo() ) )
{
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief main operation for creating a latex string.
 *
 * @details Note that this function essentially calls the base::decimal
 * methods for creating the string representation of a double three times,
 * meaning that if the central value and uncertainties are of wildly different
 * orders of magnitude, the output could look very weird.
 */
std::string
decimal::str() const
{
  if( _upper == _lower && _upper == 0 ){
    return base::decimal( _central ).dupsetting( *this ).str();
  } else {
    const std::string cen = base::decimal( _central ).dupsetting( *this ).str();
    const std::string up  = base::decimal( _upper ).dupsetting( *this ).str();
    const std::string lo  = base::decimal( _lower ).dupsetting( *this ).str();

    if( up == lo ){// string-wise comparison!
      return usr::fstr( "%s\\pm%s", cen, up );
    } else {
      return usr::fstr( "%s^{+%s}_{-%s}", cen, up, lo );
    }
  }
}

/**
 * @brief Base precision overloaded to allow for autoamtic precision setting
 *        with negative settings.
 */
decimal&
decimal::precision( const int p )
{
  if( p < 0 && ( _upper != 0 || _lower != 0 ) ){
    SetPrecision();
  } else {
    base::format::precision( abs( p ) );
  }
  return *this;
}

/**
 * @brief automatic precision setting for the decimal representation.
 * @details The precision is set such that the larger uncertain would display
 *          at least two significant digits (0 if larger uncertainty is greater
 *          than 1).
 */
void
decimal::SetPrecision()
{
  const double op_unc = std::max( _upper, _lower );
  const int exp       = GetExponent( op_unc );
  if( exp > 0 ){
    _precision = 0;
  } else {
    _precision = -exp+1;
  }
}

/*-----------------------------------------------------------------------------
 *  Scientific notation implementation functions
   --------------------------------------------------------------------------*/

/**
 * @brief allowing double input to avoid multiple interfaces for double input.
 *
 * This construction means that the measurement without uncertainty would
 * output the same values as if using a double.
 */
scientific::scientific( const double input, const unsigned p ) :
  _central( input ),
  _upper( 0 ),
  _lower( 0 ),
  _exp( 0 )
{
  precision( p );
  SetExponent();
}

/**
 * @brief simple construction with a measurement with uncertainties.
 */
scientific::scientific( const Measurement& input, const int p ) :
  _central( input.CentralValue() ),
  _upper( input.AbsUpperError() ),
  _lower( input.AbsLowerError() ),
  _exp( 0 )
{
  SetExponent();
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar instance
 */
scientific::scientific( const RooRealVar& input, const int p ) :
  _central( input.getVal() ),
  _upper( fabs( input.getErrorHi() ) ),
  _lower( fabs( input.getErrorLo() ) ),
  _exp( 0 )
{
  SetExponent();
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar instance
 */
scientific::scientific( const RooRealVar* input, const int p ) :
  _central( input->getVal() ),
  _upper( fabs( input->getErrorHi() ) ),
  _lower( fabs( input->getErrorLo() ) ),
  _exp( 0 )
{
  SetExponent();
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief implementing the virtual function.
 *
 * The function attempts to use the simplest form to represent the uncertainty,
 * using as little latex symbols as possible.
 */
std::string
scientific::str() const
{
  const std::string cen  = base::decimal( _central ).dupsetting( *this ).str();
  const std::string up   = base::decimal( _upper ).dupsetting( *this ).str();
  const std::string lo   = decimal( _lower ).dupsetting( *this ).str();
  const std::string base =
    ( up == lo && _upper == 0 ) ? cen :
    ( up == lo )                ? usr::fstr( "%s\\pm%s", cen, up ) :
    usr::fstr( "%s^{+%s}_{-%s}", cen, up, lo );

  const std::string ans =
    ( _exp == 0 )               ? base :
    ( up == lo && _upper != 0 ) ? usr::fstr( "(%s)\\times10^{%d}", base, _exp ) :
    usr::fstr( "%s\\times10^{%d}", base, _exp );

  return ans;
}

/**
 * @brief overloading base implementation to allow for precision autodetection
 *        with a negative input.
 */
scientific&
scientific::precision( const int i )
{
  if( i < 0 && ( _upper != 0 || _lower != 0 ) ){
    SetPrecision();
  } else {
    base::format::precision( abs( i ) );
  }
  return *this;
}

/**
 * @brief reducing/magnifying the central value and uncertainties by a common
 *        exponent value.
 *
 * If the central value is none-zero, then extracting the exponent of the
 * central value such that \f$1 < |\mathrm{central}_\mathrm{man}| < 10\f$;
 * If the central value is zero, then the exponent is determined by the
 * larger of the uncertainties, if it is non-zero.
 * If everything is zero, then the exponent is not set.
 */
void
scientific::SetExponent()
{
  if( _central != 0 ){
    _exp      = GetExponent( _central );
    _central /= IntPower( 10, _exp );
    _upper   /= IntPower( 10, _exp );
    _lower   /= IntPower( 10, _exp );
  } else if( _upper != 0 || _lower != 0  ){
    _exp      = std::max( GetExponent( _upper ), GetExponent( _lower ) );
    _central /= IntPower( 10, _exp );
    _upper   /= IntPower( 10, _exp );
    _lower   /= IntPower( 10, _exp );
  }
}

/**
 * @brief automatically determining the precision to use.
 *
 * If the central value is smaller than the larger uncertainty, then at most
 * the precision is 1 (if the uncertainty after the exponent has been factored
 * out smaller than 10).
 * if the central value is larger than the larger uncertainty, the precision
 * is set such that two significant digits of the larger uncertainty is
 * displayed.
 */
void
scientific::SetPrecision()
{
  double op_unc = std::max( _upper, _lower );
  if( op_unc > _central ){
    if( op_unc < 10 ){
      _precision = 1;
    } else {
      _precision = 0;
    }
  } else {
    _precision = 1;

    while( op_unc < 1 && op_unc > 0 ){
      _precision++;
      op_unc *= 10;
    }
  }
}

}/* fmt */

/**
 * @brief template specialization for a json file parsing.
 *
 * reading a list of doubles with 1-3 parameters as a measurement class.
 * * If only an empty list exists at the address, then the unit measurement
 *   (1+-0), * is returned.
 * * If only the list is only a single double long, a measurement with no
 *   uncertainty is returned.
 * * If only two double exist, then a measurement with symmetric uncertainty is
 *   returned. (x[0] +- x[1])
 * * If more than three doubles exists, then the first three doubles will be
 *   used to construct a measurement with asymmetric uncertainty:
 *   (x[0] +x[1] -x[2] )
 */
template<>
void
ExceptJSONEntry<Measurement>( const JSONMap&     map,
                              const std::string& index )
{
  ExceptJSONList( map, index );
}

template<>
Measurement
JSONEntry<Measurement>( const JSONMap&     map,
                        const std::string& index )
{
  const auto array = JSONList<double>( map, index );
  if( array.size() == 0 ){
    throw std::invalid_argument( "Array for usr::Measurement cannot be empty" );
  } else if( array.size() == 1 ){
    return usr::Measurement( array.at( 0 ), 0, 0 );
  } else if( array.size() == 2 ){
    return usr::Measurement( array.at( 0 ), array.at( 1 ), array.at( 1 ) );
  } else {
    return usr::Measurement( array.at( 0 ), array.at( 1 ), array.at( 2 ) );
  }
}

template<>
Measurement
JSONEntry<Measurement>( const JSONMap&     map,
                        const std::string& index,
                        const Measurement& def  )
{
  if( map.HasMember( index.c_str() ) ){
    return JSONEntry<Measurement>( map, index );
  } else {
    return def;
  }
}

}/* usr */


#include <El.hpp>

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

void read_text_block(El::DistMatrix<El::BigFloat> &block,
                     const boost::filesystem::path &block_path)
{
  boost::filesystem::ifstream block_stream(block_path);
  if(!block_stream)
    {
      throw std::runtime_error("Unable to open checkpoint file: '"
                               + block_path.string() + "'");
    }
  int64_t file_height, file_width;
  block_stream >> file_height >> file_width;
  if(!block_stream.good())
    {
      throw std::runtime_error("Corrupted header in file: "
                               + block_path.string());
    }
  if(file_height != block.Height() || file_width != block.Width())
    {
      std::stringstream ss;
      ss << "Incompatible checkpoint file: '" << block_path.string()
         << "'.  Expected dimensions (" << block.Height() << ","
         << block.Width() << "), but found (" << file_height << ","
         << file_width << ")";
      throw std::runtime_error(ss.str());
    }

  std::string element;
  for(int64_t row = 0; row < file_height; ++row)
    for(int64_t column = 0; column < file_width; ++column)
      {
        block_stream >> element;
        if(block.IsLocal(row, column))
          {
            block.SetLocal(block.LocalRow(row), block.LocalCol(column),
                           El::BigFloat(element));
          }
      }
  if(!block_stream.good())
    {
      throw std::runtime_error("Corrupted data in file: "
                               + block_path.string());
    }
}

void read_text_block(El::DistMatrix<El::BigFloat> &block,
                     const boost::filesystem::path &checkpoint_directory,
                     const std::string &prefix, const size_t &block_index)
{
  read_text_block(block, checkpoint_directory
                           / (prefix + std::to_string(block_index) + ".txt"));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2017, Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_HAS_SPIKES_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_HAS_SPIKES_HPP

#include <algorithm>

#include <boost/core/ignore_unused.hpp>
#include <boost/range.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/policies/is_valid/default_policy.hpp>

#include <boost/geometry/util/range.hpp>

#include <boost/geometry/views/closeable_view.hpp>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/validity_failure_type.hpp>
#include <boost/geometry/algorithms/detail/point_is_spike_or_equal.hpp>
#include <boost/geometry/io/dsv/write.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

template <typename Point>
struct equal_to
{
    Point const& m_point;

    equal_to(Point const& point)
        : m_point(point)
    {}

    template <typename OtherPoint>
    inline bool operator()(OtherPoint const& other) const
    {
        return geometry::equals(m_point, other);
    }
};

template <typename Point>
struct not_equal_to
{
    Point const& m_point;

    not_equal_to(Point const& point)
        : m_point(point)
    {}

    template <typename OtherPoint>
    inline bool operator()(OtherPoint const& other) const
    {
        return ! geometry::equals(other, m_point);
    }
};

template <typename Range, closure_selector Closure>
struct has_spikes
{
    template <typename Iterator>
    static inline Iterator find_different_from_first(Iterator first,
                                                     Iterator last)
    {
        typedef not_equal_to<typename point_type<Range>::type> not_equal;

        BOOST_GEOMETRY_ASSERT(first != last);

        Iterator second = first;
        ++second;
        return std::find_if(second, last, not_equal(*first));
    }

    template <typename VisitPolicy, typename SideStrategy>
    static inline bool apply(Range const& range, VisitPolicy& visitor,
                             SideStrategy const& strategy)
    {
        boost::ignore_unused(visitor);

        typedef typename closeable_view<Range const, Closure>::type view_type;
        typedef typename boost::range_iterator<view_type const>::type iterator;

        bool const is_linear
            = boost::is_same<typename tag<Range>::type, linestring_tag>::value;

        view_type const view(range);

        iterator prev = boost::begin(view);

        iterator cur = find_different_from_first(prev, boost::end(view));
        if (cur == boost::end(view))
        {
            // the range has only one distinct point, so it
            // cannot have a spike
            return ! visitor.template apply<no_failure>();
        }

        iterator next = find_different_from_first(cur, boost::end(view));
        if (next == boost::end(view))
        {
            // the range has only two distinct points, so it
            // cannot have a spike
            return ! visitor.template apply<no_failure>();
        }

        while (next != boost::end(view))
        {
            if ( geometry::detail::point_is_spike_or_equal(*prev, *next, *cur,
                                                           strategy) )
            {
                return
                    ! visitor.template apply<failure_spikes>(is_linear, *cur);
            }
            prev = cur;
            cur = next;
            next = find_different_from_first(cur, boost::end(view));
        }

        if (geometry::equals(range::front(view), range::back(view)))
        {
            iterator cur = boost::begin(view);
            typename boost::range_reverse_iterator
                <
                    view_type const
                >::type prev = find_different_from_first(boost::rbegin(view),
                                                         boost::rend(view));

            iterator next = find_different_from_first(cur, boost::end(view));
            if (detail::point_is_spike_or_equal(*prev, *next, *cur, strategy))
            {
                return
                    ! visitor.template apply<failure_spikes>(is_linear, *cur);
            }
            else
            {
                return ! visitor.template apply<no_failure>();
            }
        }

        return ! visitor.template apply<no_failure>();
    }
};

}} // namespace detail::is_valid
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_HAS_SPIKES_HPP


#include <cstdint>
#include <iostream>
#include <vector>
#include <memory>
#include <boost/thread/thread.hpp>
#include <boost/timer/timer.hpp>

const int N = 1000000000;
const int NTHREADS = boost::thread::hardware_concurrency();
const int N_PER_THREAD = N/NTHREADS;

class Adder
{
    friend inline std::uint64_t operator+(std::uint64_t lhs, const Adder& rhs);

    private:
        std::uint64_t sum = 0;
        int start, stop;

    public:
        Adder(int start, int stop) : start(start), stop(stop) {}

        void operator()()
        {
            for (int i = start; i < stop; ++i)
                sum += i;
        }
};

inline std::uint64_t operator+(std::uint64_t lhs, const Adder& rhs)
{
    return lhs + rhs.sum;
}

int main(int argc, char** argv)
{
    std::uint64_t sum = 0;

    boost::timer::cpu_timer st_timer;

    for (int i = 0; i <= N; ++i)
        sum += i;

    std::cout << "Single thread calculated sum of " << sum << " in\n";
    std::cout << st_timer.format() << '\n';

    sum = 0;
    boost::timer::cpu_timer mt_timer;

    boost::thread_group threads;
    std::vector<std::unique_ptr<Adder> > adders;
    adders.reserve(NTHREADS);
    for (int i = 0; i < NTHREADS; ++i)
    {
        int start = i * N_PER_THREAD;
        int stop = i == NTHREADS - 1 ? N + 1 : (i + 1) * N_PER_THREAD;
        adders.emplace_back(new Adder(start, stop));
        threads.add_thread(new boost::thread(boost::ref(*adders[i])));
    }
    threads.join_all();
    for (int i = 0; i < NTHREADS; ++i)
        sum = sum + *adders[i];

    std::cout << NTHREADS << " threads calculated sum of " << sum << " in\n";
    std::cout << mt_timer.format() << '\n';
    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::uint8_t, 5, 4> test_uchar_5_4(const Eigen::Matrix<boost::uint8_t, 5, 4> & M)
{
	return M;
}
void export_uchar_5_4()
{
	boost::python::def("test_uchar_5_4",test_uchar_5_4);
}