/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkDiffusionFunctionCollection.h"
#include "mitkNumericTypes.h"

#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/version.hpp>
#include <itkPointShell.h>
#include "itkVectorContainer.h"
#include "vnl/vnl_vector.h"
#include <vtkBox.h>
#include <vtkMath.h>
#include <itksys/SystemTools.hxx>
#include <boost/algorithm/string.hpp>

// Intersect a finite line (with end points p0 and p1) with all of the
// cells of a vtkImageData
std::vector< std::pair< itk::Index<3>, double > > mitk::imv::IntersectImage(const itk::Vector<double,3>& spacing, itk::Index<3>& si, itk::Index<3>& ei, itk::ContinuousIndex<float, 3>& sf, itk::ContinuousIndex<float, 3>& ef)
{
  std::vector< std::pair< itk::Index<3>, double > > out;
  if (si == ei)
  {
    double d[3];
    for (int i=0; i<3; ++i)
      d[i] = (sf[i]-ef[i])*spacing[i];
    double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );
    out.push_back(  std::pair< itk::Index<3>, double >(si, len) );
    return out;
  }

  double bounds[6];

  double entrancePoint[3];
  double exitPoint[3];

  double startPoint[3];
  double endPoint[3];

  double t0, t1;
  for (int i=0; i<3; ++i)
  {
    startPoint[i] = sf[i];
    endPoint[i] = ef[i];

    if (si[i]>ei[i])
    {
      int t = si[i];
      si[i] = ei[i];
      ei[i] = t;
    }
  }

  for (int x = si[0]; x<=ei[0]; ++x)
    for (int y = si[1]; y<=ei[1]; ++y)
      for (int z = si[2]; z<=ei[2]; ++z)
      {
        bounds[0] = (double)x - 0.5;
        bounds[1] = (double)x + 0.5;
        bounds[2] = (double)y - 0.5;
        bounds[3] = (double)y + 0.5;
        bounds[4] = (double)z - 0.5;
        bounds[5] = (double)z + 0.5;

        int entryPlane;
        int exitPlane;
        int hit = vtkBox::IntersectWithLine(bounds,
                                            startPoint,
                                            endPoint,
                                            t0,
                                            t1,
                                            entrancePoint,
                                            exitPoint,
                                            entryPlane,
                                            exitPlane);
        if (hit)
        {
          if (entryPlane>=0 && exitPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (exitPoint[i] - entrancePoint[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
          else if (entryPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (ef[i] - entrancePoint[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
          else if (exitPlane>=0)
          {
            double d[3];
            for (int i=0; i<3; ++i)
              d[i] = (exitPoint[i]-sf[i])*spacing[i];
            double len = std::sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2] );

            itk::Index<3> idx; idx[0] = x; idx[1] = y; idx[2] = z;
            out.push_back(  std::pair< itk::Index<3>, double >(idx, len) );
          }
        }
      }
  return out;
}

//------------------------- SH-function ------------------------------------

double mitk::sh::factorial(int number) {
  if(number <= 1) return 1;
  double result = 1.0;
  for(int i=1; i<=number; i++)
    result *= i;
  return result;
}

void mitk::sh::Cart2Sph(double x, double y, double z, double *spherical)
{
  double phi, th, rad;
  rad = sqrt(x*x+y*y+z*z);
  if( rad < mitk::eps )
  {
    th = itk::Math::pi/2;
    phi = itk::Math::pi/2;
  }
  else
  {
    th = acos(z/rad);
    phi = atan2(y, x);
  }
  spherical[0] = phi;
  spherical[1] = th;
  spherical[2] = rad;
}

vnl_vector_fixed<double, 3> mitk::sh::Sph2Cart(const double& theta, const double& phi, const double& rad)
{
  vnl_vector_fixed<double, 3> dir;
  dir[0] = rad * sin(theta) * cos(phi);
  dir[1] = rad * sin(theta) * sin(phi);
  dir[2] = rad * cos(theta);
  return dir;
}

double mitk::sh::legendre0(int l)
{
  if( l%2 != 0 )
  {
    return 0;
  }
  else
  {
    double prod1 = 1.0;
    for(int i=1;i<l;i+=2) prod1 *= i;
    double prod2 = 1.0;
    for(int i=2;i<=l;i+=2) prod2 *= i;
    return pow(-1.0,l/2.0)*(prod1/prod2);
  }
}

double mitk::sh::Yj(int m, int l, float theta, float phi, bool mrtrix)
{
  if (!mrtrix)
  {
    if (m<0)
      return sqrt(2.0)*::boost::math::spherical_harmonic_r(l, -m, theta, phi);
    else if (m==0)
      return ::boost::math::spherical_harmonic_r(l, m, theta, phi);
    else
      return pow(-1.0,m)*sqrt(2.0)*::boost::math::spherical_harmonic_i(l, m, theta, phi);
  }
  else
  {
    double plm = ::boost::math::legendre_p<float>(l,abs(m),-cos(theta));
    double mag = sqrt((double)(2*l+1)/(4.0*itk::Math::pi)*::boost::math::factorial<float>(l-abs(m))/::boost::math::factorial<float>(l+abs(m)))*plm;
    if (m>0)
      return mag*cos(m*phi);
    else if (m==0)
      return mag;
    else
      return mag*sin(-m*phi);
  }

  return 0;
}

vnl_matrix<float> mitk::sh::CalcShBasisForDirections(int sh_order, vnl_matrix<double> U, bool mrtrix)
{
  vnl_matrix<float> sh_basis  = vnl_matrix<float>(U.cols(), (sh_order*sh_order + sh_order + 2)/2 + sh_order );
  for(unsigned int i=0; i<U.cols(); i++)
  {
    double x = U(0,i);
    double y = U(1,i);
    double z = U(2,i);
    double spherical[3];
    mitk::sh::Cart2Sph(x,y,z,spherical);
    U(0,i) = spherical[0];
    U(1,i) = spherical[1];
    U(2,i) = spherical[2];
  }

  for(unsigned int i=0; i<U.cols(); i++)
  {
    for(int k=0; k<=sh_order; k+=2)
    {
      for(int m=-k; m<=k; m++)
      {
        int j = (k*k + k + 2)/2 + m - 1;
        double phi = U(0,i);
        double th = U(1,i);
        sh_basis(i,j) = mitk::sh::Yj(m,k,th,phi, mrtrix);
      }
    }
  }

  return sh_basis;
}

float mitk::sh::GetValue(const vnl_vector<float> &coefficients, const int &sh_order, const double theta, const double phi, const bool mrtrix)
{
  float val = 0;
  for(int k=0; k<=sh_order; k+=2)
  {
    for(int m=-k; m<=k; m++)
    {
      int j = (k*k + k + 2)/2 + m - 1;
      val += coefficients[j] * mitk::sh::Yj(m, k, theta, phi, mrtrix);
    }
  }

  return val;
}

float mitk::sh::GetValue(const vnl_vector<float> &coefficients, const int &sh_order, const vnl_vector_fixed<double, 3> &dir, const bool mrtrix)
{
  double spherical[3];
  mitk::sh::Cart2Sph(dir[0], dir[1], dir[2], spherical);

  float val = 0;
  for(int k=0; k<=sh_order; k+=2)
  {
    for(int m=-k; m<=k; m++)
    {
      int j = (k*k + k + 2)/2 + m - 1;
      val += coefficients[j] * mitk::sh::Yj(m, k, spherical[1], spherical[0], mrtrix);
    }
  }

  return val;
}

//------------------------- gradients-function ------------------------------------

mitk::gradients::GradientDirectionContainerType::Pointer mitk::gradients::ReadBvalsBvecs(std::string bvals_file, std::string bvecs_file, double& reference_bval)
{
  mitk::gradients::GradientDirectionContainerType::Pointer directioncontainer = mitk::gradients::GradientDirectionContainerType::New();

  std::vector<float> bvec_entries;
  if (!itksys::SystemTools::FileExists(bvecs_file))
    mitkThrow() << "bvecs file not existing: " << bvecs_file;
  else
  {
    std::string line;
    std::ifstream myfile (bvecs_file.c_str());
    if (myfile.is_open())
    {
      while (std::getline(myfile, line))
      {
        std::vector<std::string> strs;
        boost::split(strs,line,boost::is_any_of("\t \n"));
        for (auto token : strs)
        {
          if (!token.empty())
          {
            try
            {
              bvec_entries.push_back(boost::lexical_cast<float>(token));
            }
            catch(...)
            {
              mitkThrow() << "Encountered invalid bvecs file entry >" << token << "<";
            }
          }
        }
      }
      myfile.close();
    }
    else
    {
      mitkThrow() << "bvecs file could not be opened: " << bvals_file;
    }
  }

  reference_bval = -1;
  std::vector<float> bval_entries;
  if (!itksys::SystemTools::FileExists(bvals_file))
    mitkThrow() << "bvals file not existing: " << bvals_file;
  else
  {
    std::string line;
    std::ifstream myfile (bvals_file.c_str());
    if (myfile.is_open())
    {
      while (std::getline(myfile, line))
      {
        std::vector<std::string> strs;
        boost::split(strs,line,boost::is_any_of("\t \n"));
        for (auto token : strs)
        {
          if (!token.empty())
          {
            try {
              bval_entries.push_back(boost::lexical_cast<float>(token));
              if (bval_entries.back()>reference_bval)
                reference_bval = bval_entries.back();
            }
            catch(...)
            {
              mitkThrow() << "Encountered invalid bvals file entry >" << token << "<";
            }
          }
        }
      }
      myfile.close();
    }
    else
    {
      mitkThrow() << "bvals file could not be opened: " << bvals_file;
    }
  }

  for(unsigned int i=0; i<bval_entries.size(); i++)
  {
    double b_val = bval_entries.at(i);

    mitk::gradients::GradientDirectionType vec;
    vec[0] = bvec_entries.at(i);
    vec[1] = bvec_entries.at(i+bval_entries.size());
    vec[2] = bvec_entries.at(i+2*bval_entries.size());

    // Adjust the vector length to encode gradient strength
    double factor = b_val/reference_bval;
    if(vec.magnitude() > 0)
    {
      vec.normalize();
      vec[0] = sqrt(factor)*vec[0];
      vec[1] = sqrt(factor)*vec[1];
      vec[2] = sqrt(factor)*vec[2];
    }

    directioncontainer->InsertElement(i,vec);
  }

  return directioncontainer;
}

void mitk::gradients::WriteBvalsBvecs(std::string bvals_file, std::string bvecs_file, GradientDirectionContainerType::Pointer gradients, double reference_bval)
{
  std::ofstream myfile;
  myfile.open (bvals_file.c_str());
  for(unsigned int i=0; i<gradients->Size(); i++)
  {
    double twonorm = gradients->ElementAt(i).two_norm();
    myfile << std::round(reference_bval*twonorm*twonorm) << " ";
  }
  myfile.close();

  std::ofstream myfile2;
  myfile2.open (bvecs_file.c_str());
  for(int j=0; j<3; j++)
  {
    for(unsigned int i=0; i<gradients->Size(); i++)
    {
      GradientDirectionType direction = gradients->ElementAt(i);
      direction.normalize();
      myfile2 << direction.get(j) << " ";
    }
    myfile2 << std::endl;
  }
}

std::vector<unsigned int> mitk::gradients::GetAllUniqueDirections(const BValueMap & refBValueMap, GradientDirectionContainerType *refGradientsContainer )
{

  IndiciesVector directioncontainer;
  auto mapIterator = refBValueMap.begin();

  if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
    mapIterator++; //skip bzero Values

  for( ; mapIterator != refBValueMap.end(); mapIterator++){

    IndiciesVector currentShell = mapIterator->second;

    while(currentShell.size()>0)
    {
      unsigned int wntIndex = currentShell.back();
      currentShell.pop_back();

      auto containerIt = directioncontainer.begin();
      bool directionExist = false;
      while(containerIt != directioncontainer.end())
      {
        if (fabs(dot_product(refGradientsContainer->ElementAt(*containerIt), refGradientsContainer->ElementAt(wntIndex)))  > 0.9998)
        {
          directionExist = true;
          break;
        }
        containerIt++;
      }
      if(!directionExist)
      {
        directioncontainer.push_back(wntIndex);
      }
    }
  }

  return directioncontainer;
}

bool mitk::gradients::CheckForDifferingShellDirections(const BValueMap & refBValueMap, GradientDirectionContainerType::ConstPointer refGradientsContainer)
{
  auto mapIterator = refBValueMap.begin();

  if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
    mapIterator++; //skip bzero Values

  for( ; mapIterator != refBValueMap.end(); mapIterator++){

    auto mapIterator_2 = refBValueMap.begin();
    if(refBValueMap.find(0) != refBValueMap.end() && refBValueMap.size() > 1)
      mapIterator_2++; //skip bzero Values

    for( ; mapIterator_2 != refBValueMap.end(); mapIterator_2++){

      if(mapIterator_2 == mapIterator) continue;

      IndiciesVector currentShell = mapIterator->second;
      IndiciesVector testShell = mapIterator_2->second;
      for (unsigned int i = 0; i< currentShell.size(); i++)
        if (fabs(dot_product(refGradientsContainer->ElementAt(currentShell[i]), refGradientsContainer->ElementAt(testShell[i])))  <= 0.9998) { return true; }

    }
  }
  return false;
}

vnl_matrix<double> mitk::gradients::ComputeSphericalFromCartesian(const IndiciesVector & refShell, const GradientDirectionContainerType * refGradientsContainer)
{

  vnl_matrix<double> Q(3, refShell.size());
  Q.fill(0.0);

  for(unsigned int i = 0; i < refShell.size(); i++)
  {
    GradientDirectionType dir = refGradientsContainer->ElementAt(refShell[i]);
    double x = dir.normalize().get(0);
    double y = dir.normalize().get(1);
    double z = dir.normalize().get(2);
    double cart[3];
    mitk::sh::Cart2Sph(x,y,z,cart);
    Q(0,i) = cart[0];
    Q(1,i) = cart[1];
    Q(2,i) = cart[2];
  }
  return Q;
}

vnl_matrix<double> mitk::gradients::ComputeSphericalHarmonicsBasis(const vnl_matrix<double> & QBallReference, const unsigned int & LOrder)
{
  vnl_matrix<double> SHBasisOutput(QBallReference.cols(), (LOrder+1)*(LOrder+2)*0.5);
  SHBasisOutput.fill(0.0);
  for(int i=0; i< (int)SHBasisOutput.rows(); i++)
    for(int k = 0; k <= (int)LOrder; k += 2)
      for(int m =- k; m <= k; m++)
      {
        int j = ( k * k + k + 2 ) / 2.0 + m - 1;
        double phi = QBallReference(0,i);
        double th = QBallReference(1,i);
        double val = mitk::sh::Yj(m,k,th,phi);
        SHBasisOutput(i,j) = val;
      }
  return SHBasisOutput;
}

mitk::gradients::GradientDirectionContainerType::Pointer mitk::gradients::CreateNormalizedUniqueGradientDirectionContainer(const mitk::gradients::BValueMap & bValueMap,
                                                                                const GradientDirectionContainerType *origninalGradentcontainer)
{
  mitk::gradients::GradientDirectionContainerType::Pointer directioncontainer = mitk::gradients::GradientDirectionContainerType::New();
  auto mapIterator = bValueMap.begin();

  if(bValueMap.find(0) != bValueMap.end() && bValueMap.size() > 1){
    mapIterator++; //skip bzero Values
    vnl_vector_fixed<double, 3> vec;
    vec.fill(0.0);
    directioncontainer->push_back(vec);
  }

  for( ; mapIterator != bValueMap.end(); mapIterator++){

    IndiciesVector currentShell = mapIterator->second;

    while(currentShell.size()>0)
    {
      unsigned int wntIndex = currentShell.back();
      currentShell.pop_back();

      mitk::gradients::GradientDirectionContainerType::Iterator containerIt = directioncontainer->Begin();
      bool directionExist = false;
      while(containerIt != directioncontainer->End())
      {
        if (fabs(dot_product(containerIt.Value(), origninalGradentcontainer->ElementAt(wntIndex)))  > 0.9998)
        {
          directionExist = true;
          break;
        }
        containerIt++;
      }
      if(!directionExist)
      {
        GradientDirectionType dir(origninalGradentcontainer->ElementAt(wntIndex));
        directioncontainer->push_back(dir.normalize());
      }
    }
  }

  return directioncontainer;
}


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <arrayfire.h>
#include <gauss/clustering.h>
#include <gauss/internal/scopedHostPtr.h>
#include <gauss/normalization.h>
#include <gauss/linalg.h>
#include <gauss/random.h>

#include <Eigen/Eigenvalues>
#include <limits>
#include <random>
#include <tuple>
#include <iostream>
#include <chrono>

namespace gauss::clustering
{
    /**
     * Computes initial k means or centroids.
     *
     * @param tts       The time series.
     * @param k         The number of centroids.
     * @return          The new centroids.
     */
    af::array calculateInitialMeans(const af::array &tss, int k)
    {
        return af::constant(0, tss.dims(0), k, tss.type());
    }

    /**
     * Computes The euclidean distance for a tiled time series agains k-means.
     *
     * @param tts       The tiled time series.
     * @param means     The centroids.
     * @return          The distance from a time series to all k-means.
     */
    af::array kEuclideanDistance(const af::array &tts, const af::array &means)
    {
        return af::reorder(af::sqrt(af::sum(af::pow((tts - means), 2), 0)), 1, 0);
    }

    /**
     * Computes the euclidean distance of each time series w.r.t. all k-means.
     *
     * @param tss           The time series.
     * @param means         The k-means.
     * @param minDistance   The resulting distance for each time series to all k-means.
     * @param labels        The ids of the closes mean for all time series.
     */
    void euclideanDistance(const af::array &tss, const af::array &means, af::array &minDistance, af::array &idxs)
    {
        auto nSeries = tss.dims(1);
        af::array kDistances = af::constant(0.0, means.dims(1), nSeries, tss.type());

        // This for loop could be parallel, not parallelized to keep memory footprint low
        for (int i = 0; i < nSeries; i++)
        {
            af::array tiledSeries = af::tile(tss.col(i), 1, means.dims(1));
            kDistances(af::span, i) = kEuclideanDistance(tiledSeries, means);
        }
        af::min(minDistance, idxs, kDistances, 0);
    }

    /**
     * Compute the new means for the i-th iteration.
     *
     * @param tss       The time series.
     * @param labels    The ids for each time series which indicates the closest mean.
     * @param k         Number of means.
     * @return          The new means.
     */
    af::array computeNewMeans(const af::array &tss, const af::array &labels, int k)
    {
        af::array labelsTiled = af::tile(labels, tss.dims(0));
        af::array newMeans = af::constant(0.0, tss.dims(0), k, tss.type());

        gfor(af::seq ii, k)
        {
            newMeans(af::span, ii) = af::sum(tss * (labelsTiled == ii), 1) / (af::sum(af::sum((labels == ii), 3), 1));
        }
        return newMeans;
    }

    /**
     *  This function generates random labels for n time series.
     *
     * @param nTimeSeries   Number of time series to be labeled.
     * @param k             The number of groups.
     * @return              The random labels.
     */
    af::array generateRandomLabels(int nTimeSeries, int k)
    {
        std::vector<int> idx(nTimeSeries, 0);

        // Fill with sequential data
        for (int i = 0; i < nTimeSeries; i++)
        {
            idx[i] = i % k;
        }

        // Randomize
        std::shuffle(idx.begin(), idx.end(), std::mt19937(std::random_device()()));
        return af::array(nTimeSeries, 1, idx.data());
    }

    /**
     * Computes the means' difference between two iterations.
     *
     * @param means     The last iteration means
     * @param newMeans  The newMeans
     * @return          The accumulated change ratio between iterations.
     */
    float computeError(const af::array &means, const af::array &newMeans)
    {
        auto err = af::sum(af::sqrt(af::sum(af::pow(means - newMeans, 2), 0)));
        if (err.type() != af::dtype::f32)
            err = err.as(af::dtype::f32);
        return err.scalar<float>();
    }

    void kMeans(const af::array &tss, int k, af::array &centroids, af::array &labels, float tolerance, int maxIterations)
    {
        float error = std::numeric_limits<float>::max();

        if (centroids.isempty())
        {
            // initial guess of means, select k random time series
            centroids = calculateInitialMeans(tss, k);
        }

        if (labels.isempty())
        {
            // assigns a random centroid to every time series
            labels = generateRandomLabels(tss.dims(1), k);
        }

        af::array distances = af::constant(0, tss.dims(1), tss.type());
        af::array newMeans;
        int iter = 0;

        // Stop updating after convergence is reached.
        while ((error > tolerance) && (iter < maxIterations))
        {
            // 1. Compute distances to current means
            euclideanDistance(tss, centroids, distances, labels);

            // 2. Compute new means
            newMeans = computeNewMeans(tss, labels, k);

            // 3. Compute convergence
            error = computeError(centroids, newMeans);

            // 4. Update Means
            centroids = newMeans;
            iter++;
        }
    }

    //////////////////
    // K-Shape
    //////////////////

    /**
     * Computes the normalized crosscorrelation for all time series and all centroids.
     *
     * @param tss       The set of time series.
     * @param centroids The set of centroids.
     * @return          The computed normalized CrossCorrelation.
     */
    af::array ncc3Dim(const af::array &tss, const af::array &centroids) {
        auto normtss = af::sqrt(af::sum(af::pow(tss, 2.0), 0));
        auto normcen = af::sqrt(af::sum(af::pow(centroids, 2.0), 0));
        auto den = af::matmulTN(normcen, normtss);
        auto den_tiled = af::tile(den, 1, 1, (centroids.dims(0)*2)-1);

        auto inv_centroids = af::flip(centroids, 0);
        auto batched = af::reorder(inv_centroids, 0, 2, 1, 3);
        auto convolution = af::convolve1(tss, batched, AF_CONV_EXPAND);
        auto shaped = af::reorder(convolution, 2, 1, 0);
        return shaped / den_tiled;
    }

    /**
     * This function computes the assignment step. It is the update of time series labels w.r.t. the dinamics of the
     * centroids.
     *
     * @param tss       The set of time series in columnar manner.
     * @param centroids The set of centroids in columnar mode.
     * @return          The new set of labels.
     */
    af::array assignmentStep(const af::array &tss, const af::array &centroids)
    {
        auto distances = 1.0 - af::max(ncc3Dim(tss, centroids), 2);
        af::array min;
        af::array labels;
        af::min(min, labels, distances, 0);
        return labels.T();
    }

    /**
     * This function returns an updated shape of the centroid passed as argument w.r.t. the tss.
     *
     * @param tss       The subset of time series acting on the centroid.  These are znorm.
     * @param centroid  The given centroid (which could be all zeros in the first iteration or
     *                  znorm centroid in further iterations).
     * @return          The updated shape of the centroid znorm.
     */
    af::array shapeExtraction(const af::array &tss, const af::array &centroid, const af::array &p)
    {
        // since data is always znorm and so are the centroids,
        // it is not necessary to invoke sbd, ever!
        auto s = af::matmulNT(tss, tss);
        auto m = af::matmul(p, s, p);
        af::array eigvec;
        std::tie (std::ignore, eigvec) = gauss::linalg::eigh(m);
        // last column, per eigh, is the eigenvector whose
        // eigenval is max.
        auto c = eigvec.col(af::end);
        auto z_c = gauss::normalization::znorm(c, 0, 1);

        auto findDistance1 = af::sqrt(af::sum(af::pow((tss.col(0) - c), 2.0)));
        auto findDistance2 = af::sqrt(af::sum(af::pow((tss.col(0) + c), 2.0)));
        auto condition = findDistance1 >= findDistance2;

        // use select instead of 'if'
        auto condition_tied = af::tile(condition, tss.dims(0), 1);
        return af::select(condition_tied, -1.0 * z_c, z_c);
    }

    /**
     * This function performs the refinement step.
     *
     * @param tss       The set of time series in columnar manner.
     * @param centroids The set of centroids in columnar mode.
     * @param labels    The set of labels.
     * @return          The new centroids.
     */
    af::array refinementStep(const af::array &tss, const af::array &centroids, const af::array &labels)
    {
        auto ncentroids = centroids.dims(1);
        af::array result = centroids;

        // prepare p array
        // this used to be @ shapeExtraction method but it is a constant throughout
        // the entire process...
        auto nelements = tss.dims(0);
        auto scale = 1.0 / static_cast<double>(nelements);
        auto scale_tiled = af::constant(scale, nelements, nelements, tss.type());
        auto p = af::identity(nelements, nelements, tss.type()) - scale_tiled;

        for (dim_t j = 0; j < ncentroids; j++) {
            // current centroid
            auto centroid = centroids.col(j);
            // select those tss assigned to the centroid j
            auto subset = af::lookup(tss, af::where((labels == j)), 1);
            // if centroid j has at least one labeled time series.
            if (!subset.isempty()) {
                result(af::span, j) = shapeExtraction(subset, centroid, p);
            }
        }

        return result;
    }

    void kshape_calibrate(const af::array &tss, int k, af::array &centroids, af::array &labels, const int maxIterations, const bool rnd_labels)
    {
        auto nTimeseries = static_cast<unsigned int>(tss.dims(1));
        auto nElements = static_cast<unsigned int>(tss.dims(0));

        if (centroids.isempty()) {
            centroids = af::constant(0, nElements, k, tss.type());
        }

        if (labels.isempty()) {
            labels = rnd_labels
                ? gauss::random::randint(0, k, af::dim4(nTimeseries), af::dtype::u32)
                : af::iota(af::dim4(nTimeseries), af::dim4(1), af::dtype::u32) % k;
        }
        else {
            if (labels.dims(0) != nTimeseries)
                throw std::invalid_argument("The number of labels must be equal to the number of time series");

            if (labels.type() != af::dtype::u32)
                labels = labels.as(af::dtype::u32);

            auto unique_labels = af::setUnique(labels, false);
            if (unique_labels.dims(0) != k)
                throw std::invalid_argument("The number of unique labels do not correspond to the number of clusters");

            if (!af::allTrue<bool>(unique_labels >= 0 && unique_labels < k))
                throw std::invalid_argument("The labels should be identified with values ranging from 0 up to the number of clusters");
        }

        auto terminate = false;
        int iter = 0;

        // 0. Ensure tss is normalized
        auto normTSS = gauss::normalization::znorm(tss, 0, 1);

        while (!terminate)
        {
            // 1. Refinement step. New centroids computation.
            auto newCentroids = refinementStep(normTSS, centroids, labels);

            // 2. Assignment step. New labels computation.
            auto new_labels = assignmentStep(normTSS, newCentroids);

            // 3. Update centroids
            centroids = newCentroids;

            // 4. Check if no movement in labels or max iterations reached
            terminate = iter++ == maxIterations || af::allTrue<bool>(new_labels == labels);

            // 5. Update labels.
            labels = new_labels;
        }
    }

    af::array kshape_classify(const af::array &tss, const af::array &centroids) {
        auto normTSS = gauss::normalization::znorm(tss, 0, 1);
        return assignmentStep(normTSS, centroids);
    }
}


/*
 * (C) Copyright 1996- ECMWF.
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 * In applying this licence, ECMWF does not waive the privileges and immunities
 * granted to it by virtue of its status as an intergovernmental organisation
 * nor does it submit to any jurisdiction.
 */

#include "eckit/linalg/dense/LinearAlgebraArmadillo.h"

#include <armadillo>

#include <ostream>

#include "eckit/exception/Exceptions.h"
#include "eckit/linalg/Matrix.h"
#include "eckit/linalg/SparseMatrix.h"
#include "eckit/linalg/Vector.h"

namespace eckit {
namespace linalg {
namespace dense {

static const LinearAlgebraArmadillo __la("armadillo");

using vec_t = arma::vec;
using mat_t = arma::mat;

void LinearAlgebraArmadillo::print(std::ostream& out) const {
    out << "LinearAlgebraArmadillo[]";
}

Scalar LinearAlgebraArmadillo::dot(const Vector& x, const Vector& y) const {
    ASSERT(x.size() == y.size());

    // Armadillo requires non-const pointers to the data for views without copy
    vec_t xi(const_cast<Scalar*>(x.data()), x.size(), /* copy_aux_mem= */ false);
    vec_t yi(const_cast<Scalar*>(y.data()), y.size(), /* copy_aux_mem= */ false);

    return arma::dot(xi, yi);
}

void LinearAlgebraArmadillo::gemv(const Matrix& A, const Vector& x, Vector& y) const {
    ASSERT(x.size() == A.cols());
    ASSERT(y.size() == A.rows());

    // Armadillo requires non-const pointers to the data for views without copy
    mat_t Ai(const_cast<Scalar*>(A.data()), A.rows(), A.cols(), /* copy_aux_mem= */ false);
    vec_t xi(const_cast<Scalar*>(x.data()), x.size(), /* copy_aux_mem= */ false);
    vec_t yi(y.data(), y.size(), /* copy_aux_mem= */ false);

    yi = Ai * xi;
}

void LinearAlgebraArmadillo::gemm(const Matrix& A, const Matrix& B, Matrix& C) const {
    ASSERT(A.cols() == B.rows());
    ASSERT(A.rows() == C.rows());
    ASSERT(B.cols() == C.cols());

    // Armadillo requires non-const pointers to the data for views without copy
    mat_t Ai(const_cast<Scalar*>(A.data()), A.rows(), A.cols(), /* copy_aux_mem= */ false);
    mat_t Bi(const_cast<Scalar*>(B.data()), B.rows(), B.cols(), /* copy_aux_mem= */ false);
    mat_t Ci(C.data(), C.rows(), C.cols(), /* copy_aux_mem= */ false);

    Ci = Ai * Bi;
}

}  // namespace dense
}  // namespace linalg
}  // namespace eckit


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ACCUMULATORS_PUBKEY_SSS_DEAL_SHARES_HPP
#define CRYPTO3_ACCUMULATORS_PUBKEY_SSS_DEAL_SHARES_HPP

#include <cstddef>
#include <set>
#include <utility>
#include <algorithm>
#include <iterator>

#include <boost/concept_check.hpp>

#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>

#include <nil/crypto3/pubkey/accumulators/parameters/threshold_value.hpp>
#include <nil/crypto3/pubkey/accumulators/parameters/iterator_last.hpp>
#include <nil/crypto3/pubkey/accumulators/parameters/weights.hpp>

#include <nil/crypto3/pubkey/secret_sharing/shamir.hpp>
#include <nil/crypto3/pubkey/secret_sharing/feldman.hpp>
#include <nil/crypto3/pubkey/secret_sharing/pedersen.hpp>
#include <nil/crypto3/pubkey/secret_sharing/weighted_shamir.hpp>

#include <nil/crypto3/pubkey/modes/isomorphic.hpp>

namespace nil {
    namespace crypto3 {
        namespace pubkey {
            namespace accumulators {
                namespace impl {
                    template<typename ProcessingMode, typename = void>
                    struct deal_shares_impl;

                    template<typename ProcessingMode>
                    struct deal_shares_impl<ProcessingMode> : boost::accumulators::accumulator_base {
                    protected:
                        typedef ProcessingMode processing_mode_type;
                        typedef typename processing_mode_type::scheme_type scheme_type;
                        typedef typename processing_mode_type::op_type op_type;
                        typedef typename processing_mode_type::internal_accumulator_type internal_accumulator_type;

                    public:
                        typedef typename processing_mode_type::result_type result_type;

                        //
                        // boost::accumulators::sample -- participants number
                        //
                        // nil::crypto3::accumulators::threshold_value -- threshold number of participants
                        //
                        template<typename Args>
                        deal_shares_impl(const Args &args) :
                            seen_coeffs(0), n(args[boost::accumulators::sample]),
                            t(args[nil::crypto3::accumulators::threshold_value]) {
                            if constexpr (std::is_same<weighted_shamir_sss<typename scheme_type::group_type>,
                                                       scheme_type>::value) {
                                processing_mode_type::init_accumulator(
                                    acc, n, t, args[nil::crypto3::accumulators::weights]);
                            } else {
                                processing_mode_type::init_accumulator(acc, n, t);
                            }
                        }

                        inline result_type result(boost::accumulators::dont_care) const {
                            assert(t == seen_coeffs);

                            return processing_mode_type::process(acc);
                        }

                        //
                        // boost::accumulators::sample -- polynomial coefficients
                        // input coefficients should be supplied in increasing term degrees order
                        //
                        template<typename Args>
                        inline void operator()(const Args &args) {
                            resolve_type(args[boost::accumulators::sample],
                                         args[::nil::crypto3::accumulators::iterator_last | nullptr]);
                        }

                    protected:
                        inline void resolve_type(const typename scheme_type::coeff_type &coeff,
                                                 std::nullptr_t = nullptr) {
                            if (t == seen_coeffs) {
                                return;
                            }

                            processing_mode_type::update(acc, seen_coeffs, coeff);
                            seen_coeffs++;
                        }

                        template<typename InputRange>
                        inline void resolve_type(const InputRange &range, std::nullptr_t) {
                            for (const auto &c : range) {
                                resolve_type(c);
                            }
                        }

                        template<typename InputIterator>
                        inline void resolve_type(InputIterator first, InputIterator last) {
                            for (auto it = first; it != last; it++) {
                                resolve_type(*it);
                            }
                        }

                        std::size_t n;
                        std::size_t t;
                        std::size_t seen_coeffs;
                        mutable internal_accumulator_type acc;
                    };

                    // template<typename ProcessingMode>
                    // struct deal_shares_impl<
                    //     ProcessingMode,
                    //     typename std::enable_if<std::is_same<
                    //         typename ProcessingMode::scheme_type,
                    //         pubkey::weighted_shamir_sss<typename
                    //         ProcessingMode::scheme_type::group_type>>::value>::type>
                    //     : boost::accumulators::accumulator_base {
                    // protected:
                    //     typedef typename ProcessingMode::scheme_type scheme_type;
                    //     typedef typename ProcessingMode::op_type op_type;
                    //
                    //     typedef typename op_type::coeffs_type coeffs_type;
                    //     typedef typename op_type::weight_type weight_type;
                    //     typedef typename op_type::weights_type weights_type;
                    //     typedef typename op_type::shares_type shares_type;
                    //
                    // public:
                    //     typedef shares_type result_type;
                    //
                    //     //
                    //     // boost::accumulators::sample -- participants number
                    //     //
                    //     // nil::crypto3::accumulators::threshold_value -- threshold number of participants
                    //     //
                    //     template<typename Args>
                    //     deal_shares_impl(const Args &args) : seen_coeffs(0) {
                    //         assert(op_type::check_t(args[nil::crypto3::accumulators::threshold_value],
                    //                                  args[boost::accumulators::sample]));
                    //         t = args[nil::crypto3::accumulators::threshold_value];
                    //         n = args[boost::accumulators::sample];
                    //         std::size_t i = 1;
                    //         std::generate_n(std::inserter(shares_weights, shares_weights.end()), n, [&i]() {
                    //             return weight_type(i++, 1);
                    //         });
                    //     }
                    //
                    //     inline result_type result(boost::accumulators::dont_care) const {
                    //         assert(t == seen_coeffs);
                    //         return op_type::deal_shares(coeffs, shares_weights);
                    //     }
                    //
                    //     //
                    //     // boost::accumulators::sample -- participant weight
                    //     // or
                    //     // boost::accumulators::sample -- polynomial coefficients
                    //     // input coefficients should be supplied in increasing term degrees order
                    //     //
                    //     template<typename Args>
                    //     inline void operator()(const Args &args) {
                    //         resolve_type(
                    //             args[boost::accumulators::sample],
                    //             args[::nil::crypto3::accumulators::iterator_last | typename
                    //             coeffs_type::iterator()]);
                    //     }
                    //
                    // protected:
                    //     template<typename Coeff,
                    //              typename InputIterator,
                    //              typename op_type::template check_coeff_type<Coeff> = true>
                    //     inline void resolve_type(const Coeff &coeff, InputIterator) {
                    //         assert(t > seen_coeffs);
                    //         coeffs.emplace_back(coeff);
                    //         seen_coeffs++;
                    //     }
                    //
                    //     template<typename Coeffs,
                    //              typename InputIterator,
                    //              typename op_type::template check_coeff_type<typename Coeffs::value_type> = true>
                    //     inline void resolve_type(const Coeffs &coeffs, InputIterator dont_care) {
                    //         for (const auto &c : coeffs) {
                    //             resolve_type(c, dont_care);
                    //         }
                    //     }
                    //
                    //     template<typename InputIterator,
                    //              typename op_type::template check_coeff_type<
                    //                  typename std::iterator_traits<InputIterator>::value_type> = true>
                    //     inline void resolve_type(InputIterator first, InputIterator last) {
                    //         for (auto it = first; it != last; it++) {
                    //             resolve_type(*it, last);
                    //         }
                    //     }
                    //
                    //     template<typename Weight,
                    //              typename InputIterator,
                    //              typename op_type::template check_weight_type<Weight> = true>
                    //     inline void resolve_type(const Weight &w, InputIterator) {
                    //         assert(op_type::check_weight(w, n));
                    //         shares_weights.insert_or_assign(w.first, w.second);
                    //     }
                    //
                    //     template<typename InputIterator,
                    //              typename op_type::template check_weight_type<
                    //                  typename std::iterator_traits<InputIterator>::value_type> = true>
                    //     inline void resolve_type(InputIterator first, InputIterator last) {
                    //         for (auto it = first; it != last; it++) {
                    //             resolve_type(*it, last);
                    //         }
                    //     }
                    //
                    //     std::size_t t;
                    //     std::size_t n;
                    //     std::size_t seen_coeffs;
                    //     coeffs_type coeffs;
                    //     weights_type shares_weights;
                    // };
                }    // namespace impl

                namespace tag {
                    template<typename ProcessingMode>
                    struct deal_shares : boost::accumulators::depends_on<> {
                        typedef ProcessingMode mode_type;

                        /// INTERNAL ONLY
                        ///

                        typedef boost::mpl::always<accumulators::impl::deal_shares_impl<mode_type>> impl;
                    };
                }    // namespace tag

                namespace extract {
                    template<typename ProcessingMode, typename AccumulatorSet>
                    typename boost::mpl::apply<AccumulatorSet, tag::deal_shares<ProcessingMode>>::type::result_type
                        deal_shares(const AccumulatorSet &acc) {
                        return boost::accumulators::extract_result<tag::deal_shares<ProcessingMode>>(acc);
                    }
                }    // namespace extract
            }        // namespace accumulators
        }            // namespace pubkey
    }                // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_ACCUMULATORS_PUBKEY_SSS_DEAL_SHARES_HPP


//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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_STATS_EXPONENTIAL_HPP
#define BOOST_STATS_EXPONENTIAL_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/config/no_tr1/cmath.hpp>

#ifdef BOOST_MSVC
# pragma warning(push)
# pragma warning(disable: 4702) // unreachable code (return after domain_error throw).
#endif

#include <utility>

namespace boost{ namespace math{

namespace detail{
//
// Error check:
//
template <class RealType, class Policy>
inline bool verify_lambda(const char* function, RealType l, RealType* presult, const Policy& pol)
{
   if(l <= 0)
   {
      *presult = policies::raise_domain_error<RealType>(
         function,
         "The scale parameter \"lambda\" must be > 0, but was: %1%.", l, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool verify_exp_x(const char* function, RealType x, RealType* presult, const Policy& pol)
{
   if(x < 0)
   {
      *presult = policies::raise_domain_error<RealType>(
         function,
         "The random variable must be >= 0, but was: %1%.", x, pol);
      return false;
   }
   return true;
}

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class exponential_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   exponential_distribution(RealType lambda = 1)
      : m_lambda(lambda)
   {
      RealType err;
      detail::verify_lambda("boost::math::exponential_distribution<%1%>::exponential_distribution", lambda, &err, Policy());
   } // exponential_distribution

   RealType lambda()const { return m_lambda; }

private:
   RealType m_lambda;
};

typedef exponential_distribution<double> exponential;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const exponential_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const exponential_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   using boost::math::tools::min_value;
   return std::pair<RealType, RealType>(min_value<RealType>(),  max_value<RealType>());
   // min_value<RealType>() to avoid a discontinuity at x = 0.
}

template <class RealType, class Policy>
inline RealType pdf(const exponential_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::pdf(const exponential_distribution<%1%>&, %1%)";

   RealType lambda = dist.lambda();
   RealType result;
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::verify_exp_x(function, x, &result, Policy()))
      return result;
   result = lambda * exp(-lambda * x);
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const exponential_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::cdf(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::verify_exp_x(function, x, &result, Policy()))
      return result;
   result = -boost::math::expm1(-x * lambda, Policy());

   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const exponential_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::quantile(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 0)
      return 0;
   if(p == 1)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = -boost::math::log1p(-p, Policy()) / lambda;
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<exponential_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::cdf(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = c.dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::verify_exp_x(function, c.param, &result, Policy()))
      return result;
   result = exp(-c.param * lambda);

   return result;
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<exponential_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::quantile(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = c.dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;

   RealType q = c.param;
   if(0 == detail::check_probability(function, q, &result, Policy()))
      return result;

   if(q == 1)
      return 0;
   if(q == 0)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = -log(q) / lambda;
   return result;
}

template <class RealType, class Policy>
inline RealType mean(const exponential_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda("boost::math::mean(const exponential_distribution<%1%>&)", lambda, &result, Policy()))
      return result;
   return 1 / lambda;
}

template <class RealType, class Policy>
inline RealType standard_deviation(const exponential_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda("boost::math::standard_deviation(const exponential_distribution<%1%>&)", lambda, &result, Policy()))
      return result;
   return 1 / lambda;
}

template <class RealType, class Policy>
inline RealType mode(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType median(const exponential_distribution<RealType, Policy>& dist)
{
   using boost::math::constants::ln_two;
   return ln_two<RealType>() / dist.lambda(); // ln(2) / lambda
}

template <class RealType, class Policy>
inline RealType skewness(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 2;
}

template <class RealType, class Policy>
inline RealType kurtosis(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 9;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 6;
}

} // namespace math
} // namespace boost

#ifdef BOOST_MSVC
# pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_EXPONENTIAL_HPP


#include "stdafx.h"

#ifdef USE_EIGEN
//#define EIGEN_DONT_VECTORIZE
//#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <opencv2/core/eigen.hpp>
#endif

using namespace std;
using namespace cv;

namespace cp
{
	void plotDCTKernel(string wname, bool isWait, const double* GCn, const int radius, const int order, const double G0, const double sigma)
	{
		const int size = (order) * (radius + 1);
		AutoBuffer<float> G(size);
		for (int i = 0; i < size; i++)G[i] = (float)GCn[i];
		plotDCTKernel(wname, isWait, G, radius, order, (float)G0, sigma);
	}

	void plotDCTKernel(string wname, bool isWait, const float* GCn, const int radius, const int order, const float G0, const double sigma)
	{
		Mat gauss(radius + 1, 1, CV_64F);
		cp::setGaussKernelHalf(gauss, radius, sigma, true);

		static int pre_order = 0;
		cp::Plot p;
		cv::namedWindow(wname);
		const int allIndex = order + 2;
		const int totalIndex = allIndex - 1;
		static int fk = allIndex; createTrackbar("k", wname, &fk, allIndex);
		if (pre_order != order)
		{
			pre_order = order;
			fk = allIndex;
			setTrackbarMax("k", wname, allIndex);
			setTrackbarPos("k", wname, fk);
		}

		int key = 0;
		for (int k = 0; k <= order; k++)
		{
			p.setPlotTitle(k, format("order %d", k));
		}
		p.setPlotTitle(order + 1, "total");

		if (G0 == 0.f)
		{
			while (key != 'q')
			{
				double error = 0.0;
				if (fk == allIndex)
				{
					for (int k = 0; k <= order; k++)
					{
						for (int n = -radius; n <= radius; n++)
						{
							//p.push_back(n, GCn[(order + 1) * abs(n) + order - k], k);
							p.push_back(n, GCn[(order + 1) * abs(n) + k], k);
						}
					}
					//total
					for (int n = -radius; n <= radius; n++)
					{
						float sum = 0.f;
						for (int k = 0; k <= order; k++)
						{
							sum += GCn[(order + 1) * abs(n) + k];
						}
						p.push_back(n, sum, totalIndex);
						double v = gauss.at<double>(abs(n)) - sum;
						error += v * v;
					}
				}
				else
				{
					if (fk == totalIndex)
					{
						for (int n = -radius; n <= radius; n++)
						{
							float sum = 0.f;
							for (int k = 0; k <= order; k++)
							{
								sum += GCn[(order + 1) * abs(n) + k];
							}
							p.push_back(n, sum, totalIndex);
							double v = gauss.at<double>(abs(n)) - sum;
							error += v * v;
						}
					}
					else
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, GCn[(order + 1) * abs(n) + order - fk], fk);
						}
					}
				}
				p.plot(wname, false, "", format("10log10(1/mse) %f", 10 * log10(1 / error)));
				p.clear();
				key = waitKey(1);
				if (!isWait)break;
			}
		}
		else //DCT1,5
		{
			while (key != 'q')
			{
				double error = 0.0;
				if (fk == allIndex)
				{
					for (int n = -radius; n <= radius; n++)
					{
						p.push_back(n, G0, 0);
					}
					for (int k = 0; k < order; k++)
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, GCn[order * abs(n) + k], k + 1);
						}
					}
					//total
					for (int n = -radius; n <= radius; n++)
					{
						float sum = G0;
						for (int k = 0; k < order; k++)
						{
							sum += GCn[order * abs(n) + k];
						}
						p.push_back(n, sum, totalIndex);
						double v = gauss.at<double>(abs(n)) - sum;
						error += v * v;
					}
				}
				else
				{
					if (fk == totalIndex)
					{
						for (int n = -radius; n <= radius; n++)
						{
							float sum = G0;
							for (int k = 0; k < order; k++)
							{
								sum += GCn[(order)*abs(n) + k];
							}
							p.push_back(n, sum, totalIndex);
							double v = gauss.at<double>(abs(n)) - sum;
							error += v * v;
						}
					}
					else if (fk == 0)
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, G0, 0);
						}
					}
					else
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, GCn[(order)*abs(n) + fk - 1], fk);
						}
					}
				}
				p.plot(wname, false, "", format("10log10(1/mse) %f", 10 * log10(1 / error)));

				p.clear();
				key = waitKey(1);
				if (!isWait)break;
			}
		}
	}

	void computeSpectrumGaussianClosedForm(const double sigma, const int K, const int R, const int dcttype, double* destSpect)
	{
		double omega = 0.0;
		double k0 = 0.0;
		switch (dcttype)
		{
		case 1:	omega = CV_2PI / (2.0 * R + 0.0); k0 = 0.0; break;
		case 3:	omega = CV_2PI / (2.0 * R + 2.0); k0 = 0.5; break;
		case 5:	omega = CV_2PI / (2.0 * R + 1.0); k0 = 0.0; break;
		case 7:	omega = CV_2PI / (2.0 * R + 1.0); k0 = 0.5; break;
			//default: throw "Unsupported DCT type"; break;
		}

		for (int k = 0; k <= K; k++)
		{
			double coeff = sigma * omega * (k + k0);
			destSpect[k] = 2.0 * exp(-0.5 * coeff * coeff);
		}

		if (dcttype == 1 || dcttype == 5)
			destSpect[0] = 1.0;
	}

	void computeCtWCinv(Mat& dest, const int K, const int R, const int dctType)
	{
		Mat Minv = Mat::eye(K, K, CV_64F);

		double T = 0.0;
		switch (dctType)
		{
		case 1:
		{
			Minv.at<double>(0, 0) = 0.5;
			//Minv.at<double>(K - 1, K - 1) = 0.5;
			T = (2.0 * R + 0.0); break;
		}
		case 3:
		{
			//Minv.at<double>(0, 0) = 0.5;
			T = (2.0 * R + 2.0); break;
		}
		case 5:
		{
			Minv.at<double>(0, 0) = 0.5;
			T = (2.0 * R + 1.0); break;
		}
		case 7:
		{
			//Minv.at<double>(0, 0) = 0.5;
			//Minv.at<double>(K - 1, K - 1) = 0.5;
			T = (2.0 * R + 1.0); break;
		}
		default: throw "Unsupported DCT type"; break;
		}

		if (dctType == 1)
		{
			Mat s = Mat::ones(K, 1, CV_64F);
			for (int i = 1; i < K; i += 2)s.at<double>(i) = -1.0;

			Mat Minvs = Minv * s;
			//cv::trace(Minv).val[0]=K-2+0.5+0.5
			Mat((Minv - (Minvs * Minvs.t()) / (T + (double)K - 1.0))).copyTo(dest);
		}
		else
		{
			//Mat(4/T * Minv).copyTo(dest);
			Minv.copyTo(dest);//scaling (4/T) is not required due to other rescaling.
		}
	}

	void setGaussKernelHalf(Mat& dest, const int R, const double sigma, bool isNormalize)
	{
		CV_Assert(dest.depth() == CV_64F);
		double sum = 0.0;
		const double coeff = 1.0 / (-2.0 * sigma * sigma);
		for (int n = 1; n <= R; ++n)
		{
			const double v = exp((double)(n * n) * coeff);
			//const double v = exp(-(double)(abs(u)) / (sigma));
			//int n = 1;
			//const double v = exp(-(double)pow(abs(u),(double)n) / (n*pow(sigma,double(n))));
			dest.at<double>(n, 0) = v;
			sum += v;
		}
		sum *= 2.0;
		dest.at<double>(0, 0) = 1.0;//u=0
		sum += 1.0;//u=0

		const int rend = int(10.0 * sigma);
		double eout = 0.0;
		for (int i = R + 1; i <= rend; i++)
		{
			const double v = exp(i * i * coeff);
			eout += v;
		}
		dest.at<double>(R, 0) += eout;

		if (isNormalize)
		{
			for (int n = 0; n <= R; ++n)
			{
				dest.at<double>(n, 0) /= sum;
			}
		}
	}

	//without normalize
	void generateCosKernel(double* dest, double& totalInv, const int dctType, const double* Gk, const int radius, const int order)
	{
		double k0;
		double omega;
		switch (dctType)
		{
		case 1:	omega = CV_2PI / (2.0 * radius + 0.0); k0 = 0.0; break;
		case 3:	omega = CV_2PI / (2.0 * radius + 2.0); k0 = 0.5; break;
		case 5:	omega = CV_2PI / (2.0 * radius + 1.0); k0 = 0.0; break;
		case 7:	omega = CV_2PI / (2.0 * radius + 1.0); k0 = 0.5; break;
		default: throw "Unsupported DCT type"; break;
		}

		double sum = 0.0;
		if (dctType == 1 || dctType == 5)
		{
			sum = Gk[0] * double(2 * radius + 1);//k=0
			for (int r = 0; r <= radius; ++r)
			{
				for (int k = 1; k <= order; ++k)
				{
					const double coeff = cos((k + k0) * omega * r) * Gk[k];
					dest[order * r + k - 1] = coeff;

					if (r == 0) sum += coeff;
					else sum += 2.0 * coeff;
				}
			}
		}
		else
		{
			for (int r = 0; r <= radius; ++r)
			{
				for (int k = 0; k <= order; ++k)
				{
					const double coeff = cos((k + k0) * omega * r) * Gk[k];

#ifdef COEFFICIENTS_SMALLEST_FIRST
					dest[(order + 1) * r + (order)-k] = coeff;
#else
					dest[(order + 1) * r + k] = coeff;
#endif

					if (r == 0) sum += coeff;
					else sum += 2.0 * coeff;
				}
			}
		}

		totalInv = (1.0 / sum);
	}

	bool optimizeSpectrum(const double sigma, const int K, const int R, const int dcttype, double* destSpect, const int M)
	{
		/*if (K > R)
		{
			computeSpectrumGaussianClosedForm(sigma, K, R, dcttype, destSpect);
			return false;
		}*/

		double omega, ratio, k0, n0 = 0.0;
		switch (dcttype)
		{
		case 1:	omega = CV_2PI / (2.0 * R + 0.0); ratio = 2.0 / sqrt(2.0 * R + 0.0); k0 = 0.0; break;
		case 3:	omega = CV_2PI / (2.0 * R + 2.0); ratio = 2.0 / sqrt(2.0 * R + 2.0); k0 = 0.5; break;
		case 5:	omega = CV_2PI / (2.0 * R + 1.0); ratio = 2.0 / sqrt(2.0 * R + 1.0); k0 = 0.0; break;
		case 7:	omega = CV_2PI / (2.0 * R + 1.0); ratio = 2.0 / sqrt(2.0 * R + 1.0); k0 = 0.5; break;
		default: throw "Unsupported DCT type"; break;
		}

		//kernel
		cv::Mat1d h0(R + 1, 1);//R+1
		setGaussKernelHalf(h0, R, sigma, false);

		//weight matrix
		cv::Mat1d W = cv::Mat1d::eye(R + 1, R + 1);
		W(0, 0) = 0.5;

		// DCT matrix
		cv::Mat1d C(R + 1, K + 1);
		for (int n = 0; n <= R; ++n)
		{
			for (int k = 0; k <= K; ++k)
			{
				C(n, k) = cos(omega * (k + k0) * (n + n0));
			}
		}
		C *= ratio;

		cv::Mat1d CWCinv;//K+1 x K+1
		cv::Mat1d a_ls;

#ifdef USE_OPTIMIZE_DCT_SWICH
		static int method = 1;
#ifdef USE_EIGEN
		createTrackbar("opt method", "", &method, 4);
#else
		createTrackbar("opt method", "", &method, 2);
#endif
#else
		int method = 0;//OpenCV direct
		//int method = 1;//fast method ICASSP2018 universal(but DCT1 and 7 has some bugs?)
		//int method = 2;//OpenCV SVD
		//int method = 3;//Eigen direct
		//int method = 4;//Eigen SVD
#endif
		//print_debug3(K, R, dcttype);
		switch (method)
		{
		default:
		case 0:
		{
			// (CWC)^-1
			CWCinv = (C.t() * W * C).inv(DECOMP_CHOLESKY);
			//cv::Mat1d CWCinv = (C.t() * W * C).inv(DECOMP_LU);
			// L2 minimization with moment preservation
			a_ls = CWCinv * C.t() * W * h0;
			//showMat64F(CWCinv, true);

			break;
		}
		case 1:
		{
			computeCtWCinv(CWCinv, K + 1, R + 1, dcttype);
			//showMat64F(CWCinv, true);
			a_ls = CWCinv * C.t() * W * h0;

			break;
		}
		case 2:
		{
			Mat w, u, vt;
			W(0, 0) = sqrt(0.5);
			//with weight W
			cv::SVDecomp(W * C, w, u, vt, SVD::FULL_UV);
			SVD::backSubst(w, u, vt, W * h0, a_ls);

			//without weight W
			//cv::SVDecomp(C, w, u, vt, SVD::FULL_UV);
			//SVD::backSubst(w, u, vt, h0, a_ls);
			break;
		}
		case 3:
		{
#ifdef USE_EIGEN
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> CE;
			cv::cv2eigen(C, CE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WE;
			cv::cv2eigen(W, WE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> h0E;
			cv::cv2eigen(h0, h0E);

			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> CtE;
			cv::cv2eigen(C.t(), CtE);

			Eigen::MatrixXd temp = CtE * WE * CE;
			Eigen::MatrixXd temp2 = temp.inverse() * CtE * WE * h0E;
			cv::eigen2cv(temp2, a_ls);
#endif
			break;
		}
		case 4:
		{

#ifdef USE_EIGEN
			W(0, 0) = sqrt(0.5);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> CE;
			cv::cv2eigen(C, CE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WE;
			cv::cv2eigen(W, WE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> h0E;
			cv::cv2eigen(h0, h0E);

			Eigen::BDCSVD<Eigen::MatrixXd> svd(WE * CE, Eigen::ComputeThinU | Eigen::ComputeThinV);
			Eigen::MatrixXd a = svd.solve(WE * h0E);
			cv::eigen2cv(a, a_ls);
#endif
			break;
		}
		}

		const double sp0 = (dcttype == 1 || dcttype == 5) ? 1.0 : 2.0;
		//const double v = sp0 / a_ls(0, 0);

#ifdef PRINT_RANK_DEFICIENT
		if (a_ls(0, 0) == 0)
		{
			cout << "PRINT_RANK_DEFICIENT" << endl;
			print_debug4(sigma, K, R, dcttype);
		}
#endif
		if (a_ls(0, 0) == 0)
		{
			computeSpectrumGaussianClosedForm(sigma, K, R, dcttype, destSpect);
			return false;
		}

		const double v = sp0 / a_ls(0, 0);

		for (int k = 0; k <= K; ++k)
		{
			//cout << k << ": " << spect[k] << ": ";
			destSpect[k] = a_ls(k, 0) * v;
			//destSpect[k] = a_ls(k, 0)*ratio;
			//cout << spect[k] << endl;
		}

		if (1 <= M)
		{
			cv::Mat1d a = a_ls.clone();

			//ideal gaussian moment vector
			cv::Mat1d mu(M, 1, 1.0);
			for (auto m = 1; m < M; ++m)
				mu(m, 0) = mu(m - 1, 0) * (2 * m - 1) * sigma * sigma;

			// Vandermonde
			cv::Mat1d V(R + 1, M);
			for (auto m = 0; m < M; ++m)
			{
				for (auto u = 0; u <= R; ++u)
					V(u, m) = pow(u, 2 * m);
				//V(0, m) /= 2.0; // moved from W
			}

			// Moment constraint
			cv::Mat1d U = V.t() * W * C;
			cv::Mat1d S = U * CWCinv * U.t();
			cv::Mat1d Sinv = S.inv();
			a -= CWCinv * U.t() * Sinv * (U * a - 0.5 * mu);

			//print_matrix(U, "[U]");
			//print_matrix(S, "[S]");
			//print_matrix(Sinv, "[S^-1]");
			//print_matrix(U*a - 0.5*mu, "[U*a - 0.5*mu]");

			for (auto k = 0; k <= K; ++k)
			{
				//cout <<k<<": "<< spect[k] << ": ";
				destSpect[k] = a(k, 0);
				//cout << spect[k] << endl;
			}

			if (dcttype == 1 || dcttype == 5)
				destSpect[0] = 1.0;
		}

		//std::cerr << "[Spectra]" << std::endl;
		//for (auto k = 0; k < K; ++k)
		//	std::cerr << cv::format("k=%d:  %10.8f  %10.8f", k, a_ls(k, 0), a(k, 0)) << std::endl;
		return true;
	}

	class SearchFullDCTRadius :public cp::Search1DInt
	{
		double sigma;
		int K;
		int dcttype;
		double* spect;

		bool isOptimize;

		double getErrorDCT(const double sigma, const int K, const int R, const int dcttype, const double* spect)
		{
			const int ROut = (int)ceil(9.0 * sigma);
			//const int ROut = (int)ceil(7.5 * sigma);
			//const int ROut = (int)ceil(6.0 * sigma);
			AutoBuffer<double> ans(ROut + 1);
			AutoBuffer<double> approx(ROut + 1);
			AutoBuffer<double> e2(ROut + 1);

			double sum = 1.0;
			ans[0] = 1.0;
			for (int i = ROut; i >= 1; --i)
			{
				double v = exp(i * i / (-2.0 * sigma * sigma));
				ans[i] = v;
				sum += 2.0 * v;
			}
			for (int i = 0; i <= ROut; i++)
			{
				ans[i] /= sum;
			}

			double errorTruncation = 0.0;
			for (int i = ROut; i >= R + 1; --i)
			{
				double e = ans[i];
				errorTruncation = fma(e, e, errorTruncation);
			}

			double phi, k0;//n0=0
			switch (dcttype)
			{
			case 1:	phi = CV_2PI / (2.0 * R + 0.0); k0 = 0.0; break;
			case 3:	phi = CV_2PI / (2.0 * R + 2.0); k0 = 0.5; break;
			case 5:	phi = CV_2PI / (2.0 * R + 1.0); k0 = 0.0; break;
			case 7:	phi = CV_2PI / (2.0 * R + 1.0); k0 = 0.5; break;

			default: throw "Unsupported DCT type"; break;
			}

			sum = 0.0;
			for (int i = R; i >= 1; --i)
			{
				double s = 0.0;
				for (int k = K; k >= 0; --k)
				{
					s = fma(cos(phi * (k + k0) * (i)), spect[k], s);//DCT1,3,5,7: n0 = 0.
				}
				sum += 2.0 * s;
				approx[i] = s;
			}
			{
				double s = 0.0;
				for (int k = K; k >= 0; --k)
				{
					s += spect[k];
				}
				sum += s;
				approx[0] = s;
			}
			for (int i = 0; i <= R; i++)
			{
				approx[i] /= sum;
			}

			double errorFrec = 0.0;
			for (int i = R; i >= 1; --i)
			{
				errorFrec = fma((ans[i] - approx[i]), (ans[i] - approx[i]), errorFrec);
			}
			errorFrec *= 2.0;
			errorFrec += (ans[0] - approx[0]) * (ans[0] - approx[0]);

			return errorFrec + errorTruncation;
		}

		double getError(int x)
		{
			if (isOptimize)optimizeSpectrum(sigma, K, x, dcttype, spect);
			else computeSpectrumGaussianClosedForm(sigma, K, x, dcttype, spect);

			return getErrorDCT(sigma, K, x, dcttype, spect);
		}

	public:
		SearchFullDCTRadius(const double sigma, const int K, const int dcttype, const double* spect, bool isOptimize)
		{
			this->sigma = sigma;
			this->K = K;
			this->dcttype = dcttype;
			this->spect = (double*)spect;
			this->isOptimize = isOptimize;
		}
	};

	int test_ratio(const double sigma, const int K, const int dcttype)
	{
		double a, b, c_1, c_2, c_3, c_4;
		int dest;
		if (dcttype == 1)
			b = 0.467347, c_1 = 0.0007, c_2 = -0.0277, c_3 = 0.6053, c_4 = 1.8088;
		else if (dcttype == 3)
			b = -0.16509, c_1 = 0.0005, c_2 = -0.0221, c_3 = 0.5458, c_4 = 2.1403;
		else if (dcttype == 5)
			b = 0.00696, c_1 = 0.0008, c_2 = -0.0289, c_3 = 0.6159, c_4 = 1.7929;
		else if (dcttype == 7)
			b = -0.029, c_1 = 0.0005, c_2 = -0.0217, c_3 = 0.5412, c_4 = 2.1912;
		else
		{
			cout << "This is not Sliding DCT" << endl;
			return 0;
		}
		a = c_1 * K * K * K + c_2 * K * K + c_3 * K + c_4;
		return dest = int(a * sigma + b);
	}

	int argminR_BruteForce_DCT(const double sigma, const int K, const int dcttype, const double* spect, const bool isOptimize, const bool isGoldenSelectionSearch)
	{
		//case K<=R:OK
		//case K>R:NG
		int r = argminR_ContinuousForm_DCT(sigma, K, dcttype, isGoldenSelectionSearch);

		const int rmin = max(1, (int)floor(0.6 * r));
		const int rmax = (int)ceil(1.4 * r);
		int argmin_r = 0;

		SearchFullDCTRadius s(sigma, K, dcttype, spect, isOptimize);

		//		s.plotError("test", rmin, rmax);
				//debug
				/*{
					int r_gs = s.goldenSearch(rmin, rmax);
					int r_li = s.linearSearch(rmin, rmax);
					if (r_li != r_gs)
					{
						cout << "NG: (sigma, k) = " << sigma << "," << K << endl;
						print_debug2(r_li, r_gs);
						print_debug2(rmin, rmax);
						s.plotError("error", rmin, rmax);
					}
					//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
				}*/

		if (isGoldenSelectionSearch)
		{
			argmin_r = s.goldenSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}
		else
		{
			argmin_r = s.linearSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}

		//cout << "Linear search radius is" << test_ratio(sigma, K, dcttype) << endl;
		//cout << "True number radius is" << argmin_r << endl;
		return argmin_r;
	}

#pragma region ContinuousForm
	class SearchDCTRadiusContinuousForm :public cp::Search1DInt
	{
		double sigma;
		int K;
		double* spect = nullptr;

		double getError(int x)
		{
			double T = 2.0 * x + 1.0;
			double Es = erfc(T / (2.0 * sigma));
			double Ef = erfc(sigma * CV_PI * (2.0 * K + 1.0) / T);
			return Es + Ef;
		}

	public:
		SearchDCTRadiusContinuousForm(const double sigma, const int K)
		{
			this->sigma = sigma;
			this->K = K;
		}
	};

	int argminR_ContinuousForm_DCT(const double sigma, const int K, const int dcttype, const bool isGoldenSelectionSearch)
	{
		int argmin_r = 0;

		int rmin = max(K, (int)floor(1.0 * sigma));
		int rmax = (int)ceil(10.0 * sigma);
		if (rmin > rmax)return rmin;
		SearchDCTRadiusContinuousForm s(sigma, K);

		if (isGoldenSelectionSearch)
		{
			argmin_r = s.goldenSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}
		else
		{
			argmin_r = s.linearSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}
		return argmin_r;
	}
#pragma endregion
}


/*
 * PCGSolver.cpp
 *
 *  Created on: Feb 14, 2012
 *      Author: ydjian
 */

#include <gtsam/linear/GaussianFactorGraph.h>
//#include <gtsam/inference/FactorGraph-inst.h>
//#include <gtsam/linear/FactorGraphUtil-inl.h>
//#include <gtsam/linear/JacobianFactorGraph.h>
//#include <gtsam/linear/LSPCGSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
//#include <gtsam/linear/SuiteSparseUtil.h>
//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
//#include <gsp2/gtsam-interface-sbm.h>
//#include <ydjian/tool/ThreadSafeTimer.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <stdexcept>

using namespace std;

namespace gtsam {

/*****************************************************************************/
void PCGSolverParameters::print(ostream &os) const {
  Base::print(os);
  os << "PCGSolverParameters:" <<  endl;
  preconditioner_->print(os);
}

/*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) {
  preconditioner_ = createPreconditioner(p.preconditioner_);
}

/*****************************************************************************/
VectorValues PCGSolver::optimize (
  const GaussianFactorGraph &gfg,
  const KeyInfo &keyInfo,
  const std::map<Key, Vector> &lambda,
  const VectorValues &initial)
{
  /* build preconditioner */
  preconditioner_->build(gfg, keyInfo, lambda);

  /* apply pcg */
  const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>(
        GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda),
        initial.vector(keyInfo.ordering()), parameters_);

  return buildVectorValues(sol, keyInfo);
}

/*****************************************************************************/
GaussianFactorGraphSystem::GaussianFactorGraphSystem(
    const GaussianFactorGraph &gfg,
    const Preconditioner &preconditioner,
    const KeyInfo &keyInfo,
    const std::map<Key, Vector> &lambda)
  : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {}

/*****************************************************************************/
void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
  /* implement b-Ax, assume x and r are pre-allocated */

  /* reset r to b */
  getb(r);

  /* substract A*x */
  Vector Ax = Vector::Zero(r.rows(), 1);
  multiply(x, Ax);
  r -= Ax ;
}

/*****************************************************************************/
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
  /* implement Ax, assume x and Ax are pre-allocated */

  /* reset y */
  Ax.setZero();

  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
      /* accumulate At A x*/
      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
        const Matrix Ai = jf->getA(it);
        /* this map lookup should be replaced */
        const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
        Ax.segment(entry.colstart(), entry.dim())
            += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim()));
      }
    }
    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
      /* accumulate H x */

      /* use buffer to avoid excessive table lookups */
      const size_t sz = hf->size();
      vector<Vector> y;
      y.reserve(sz);
      for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
        /* initialize y to zeros */
        y.push_back(zero(hf->getDim(it)));
      }

      for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) {
        /* retrieve the key mapping */
        const KeyInfoEntry &entry = keyInfo_.find(*j)->second;
        // xj is the input vector
        const Vector xj = x.segment(entry.colstart(), entry.dim());
        size_t idx = 0;
        for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) {
          if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj;
          else y[idx] += hf->info(i, j).knownOffDiagonal() * xj;
        }
      }

      /* accumulate to r */
      for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
        /* retrieve the key mapping */
        const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
        Ax.segment(entry.colstart(), entry.dim()) += y[i];
      }
    }
    else {
      throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
    }
  }
}

/*****************************************************************************/
void GaussianFactorGraphSystem::getb(Vector &b) const {
  /* compute rhs, assume b pre-allocated */

  /* reset */
  b.setZero();

  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
    if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
      const Vector rhs = jf->getb();
      /* accumulate At rhs */
      for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
        /* this map lookup should be replaced */
        const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
        b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
      }
    }
    else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
      /* accumulate g */
      for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
        const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
        b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
      }
    }
    else {
      throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
    }
  }
}

/**********************************************************************************/
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.transposeSolve(x, y); }

/**********************************************************************************/
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.solve(x, y); }

/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v,
                               const Ordering &ordering,
                               const map<Key, size_t>  & dimensions) {
  VectorValues result;

  DenseIndex offset = 0;
  for ( size_t i = 0 ; i < ordering.size() ; ++i ) {
    const Key &key = ordering[i];
    map<Key, size_t>::const_iterator it = dimensions.find(key);
    if ( it == dimensions.end() ) {
      throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions");
    }
    const size_t dim = it->second;
    result.insert(key, v.segment(offset, dim));
    offset += dim;
  }

  return result;
}

/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
  VectorValues result;
  BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
    result.insert(item.first, v.segment(item.second.colstart(), item.second.dim()));
  }
  return result;
}

}


#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <iostream>
#include <cmath>

#include "../include/KF.h"
#include "../include/dsho.h"
#include "../include/matern32.h"
#include "../include/ndsho.h"

using namespace Eigen;
using namespace std;

// function to read a space separated file

std::vector<double> load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ' ')) {
            //cout << std::stod(cell) << endl;
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return values;
}

int main() {

	// read file with two component damped simple harmonic oscillator simulated data
	//std::vector<double> values = load_csv("two_comp_dsho.txt");
	//cout << values.size() << endl;

	// map data to VectorXds
	//VectorXd times = Map<VectorXd, 0, InnerStride<2> > (values.data(), 10000);
	//VectorXd yi = Map<VectorXd, 0, InnerStride<2> > (values.data()+1, 10000);

  int vecsize = 10000;
	// map data to VectorXds
	VectorXd yi(vecsize);
  VectorXd times = Eigen::VectorXd::Random(vecsize);
  times.array() += 1.0;
  times.array() *= (100*0.5);
  std::sort(times.data(), times.data() + times.size());

	// this is the observational error vector
	VectorXd yerr = VectorXd::Ones(yi.size());
	yerr.array() *= 0.05;

	VectorXd y_sim;

	double varf_list[2] = {1.0, 0.1*0.1};
	for (int k=0; k<2; k++){
		for(int j=0; j < 6; j++) {
			double varf = varf_list[k];
			//auto const seed = std::random_device()();
			int seed = 123;
			std::mt19937 rng(seed);
			cout << seed << endl;
			double lambda = std::pow(2,j);

			// Simulate a Matern 3/2 model
			gpstate::matern32::Matern32Solver m32(times,yi,yerr, lambda, varf);
			m32.simulate_Matern32(y_sim, rng);

			double mean = y_sim.mean();
			Eigen::VectorXd tmp = y_sim.array()-mean;
			double variance  = tmp.dot(tmp) / y_sim.rows();
			std::cout << "var:" << variance << " " << std::to_string(k*6 + j+1) << std::endl;

			// write simulated vector to file GPtest.txt
			std::ofstream file("GPtest" + std::to_string(k*6 + j+1) + "_m32.txt");
			assert(file.is_open());
			file << "# Matern 3/2 simulated light curve" << endl;
			file << "# input parameters: " << endl;
			file << "# omega: " << lambda << endl;
			file << "# varf: " << varf  << endl;
			file << "#time y_sim" << endl;
			for(int i=0; i<times.rows(); i++)
				file <<  times(i) <<" "<< y_sim(i) << endl;

		}
	}

}


/**
 * @file envelope_model.cc
 * Reverberation envelope time series for a single combination of
 * receiver azimuth, source beam number, receiver beam number.
 */
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <usml/eigenverb/envelope_model.h>

using namespace usml::eigenverb;

//#define DEBUG_ENVELOPE

/**
 * Reserve the memory used to store the results of this calculation.
 */
envelope_model::envelope_model(
	const seq_vector* envelope_freq,
	size_t src_freq_first,
	const seq_vector* travel_time,
	double initial_time,
	double pulse_length,
	double threshold
) :
	_envelope_freq(envelope_freq),
	_src_freq_first(src_freq_first),
	_travel_time( travel_time->clone() ),
	_initial_time(initial_time),
	_pulse_length(pulse_length),
	_threshold(threshold),
	_level(travel_time->size()),
	_power(envelope_freq->size()),
	_duration(envelope_freq->size()),
	_intensity(envelope_freq->size(), travel_time->size())
{
}

/**
 * Reserve the memory used to store the results of this calculation.
 */
envelope_model::~envelope_model() {
	delete _travel_time ;
}

/**
 * Adds a single combination of source and receiver eigenverbs
 * to this time series.
 */
bool envelope_model::compute_intensity(
		const eigenverb& src_verb, const eigenverb& rcv_verb,
		const vector<double>& scatter, double xs2, double ys2 )
{
	bool ok = compute_overlap( src_verb, rcv_verb, scatter, xs2, ys2 );
	if ( !ok ) return false ;

	compute_time_series( src_verb.time, rcv_verb.time ) ;
	return true ;
}

/**
 * Compute the total power of the overlap between two eigenverbs.
 */
bool envelope_model::compute_overlap(
	const eigenverb& src_verb, const eigenverb& rcv_verb,
	const vector<double>& scatter, double xs2, double ys2 )
{
	#ifdef DEBUG_ENVELOPE
		cout << "wave_queue::compute_overlap() " << endl
			<< "\txs2=" << xs2
			<< " ys2=" << ys2
			<< " scatter=" << scatter << endl
			<< "\tsrc_verb"
			<< " t=" << src_verb.time
			<< " de=" << to_degrees(src_verb.source_de)
			<< " az=" << to_degrees(src_verb.source_az)
			<< " direction=" << to_degrees(src_verb.direction)
			<< " grazing=" << to_degrees(src_verb.grazing) << endl
			<< "\tpower=" << 10.0 * log10(src_verb.power)
			<< " length=" << sqrt(src_verb.length2)
			<< " width=" << sqrt(src_verb.width2) << endl
			<< "\tsurface=" << src_verb.surface << " bottom=" << src_verb.bottom
			<< " caustic=" << src_verb.caustic << endl
			<< "\trcv_verb"
			<< " t=" << rcv_verb.time
			<< " de=" << to_degrees(rcv_verb.source_de)
			<< " az=" << to_degrees(rcv_verb.source_az)
			<< " direction=" << to_degrees(rcv_verb.direction)
			<< " grazing=" << to_degrees(rcv_verb.grazing) << endl
			<< "\tpower=" << 10.0 * log10(rcv_verb.power)
			<< " length=" << sqrt(rcv_verb.length2)
			<< " width=" << sqrt(rcv_verb.width2) << endl
			<< "\tsurface=" << rcv_verb.surface << " bottom=" << rcv_verb.bottom
			<< " caustic=" << rcv_verb.caustic << endl;
	#endif

	// determine the relative tilt between the projected Gaussians

	const double alpha = src_verb.direction - rcv_verb.direction;
	const double cos2alpha = cos(2.0 * alpha);
	const double sin2alpha = sin(2.0 * alpha);

	// define subset of frequency dependent terms in source
    // Although the use of const_cast<> allows us to ignore the read-only
    // nature of src_verb, we are *very careful* to not write anything to it.

	range window( _src_freq_first, _src_freq_first + _envelope_freq->size() ) ;
	eigenverb& verb = const_cast<eigenverb&>( src_verb ) ;
	const vector_range< vector<double> > src_verb_power( verb.power, window ) ;

    // compute commonly used terms in the intersection of the Gaussian profiles

	const double src_sum = src_verb.length2 + src_verb.width2 ;
	const double src_diff = src_verb.length2 - src_verb.width2 ;
	const double src_prod = src_verb.length2 * src_verb.width2 ;

	const double rcv_sum = rcv_verb.length2 + rcv_verb.width2 ;
	const double rcv_diff = rcv_verb.length2 - rcv_verb.width2 ;
	const double rcv_prod = rcv_verb.length2 * rcv_verb.width2 ;

    // compute the scaling of the exponential
    // equations (26) and (28) from the paper

    double det_sr = 0.5 * ( 2.0 * ( src_prod + rcv_prod )
    		+ ( src_sum * rcv_sum ) - ( src_diff * rcv_diff ) * cos2alpha ) ;
    noalias(_power) = 0.25 * 0.5 * _pulse_length
    		* src_verb_power * rcv_verb.power * scatter ;

    // compute the power of the exponential
    // equation (28) from the paper

    const double new_prod = src_diff * cos2alpha ;
    const double kappa = -0.25 * (
  		  xs2 * ( src_sum + new_prod + 2.0 * rcv_verb.length2 )
		+ ys2 * ( src_sum - new_prod + 2.0 * rcv_verb.width2 )
		- 2.0 * sqrt( xs2 * ys2 ) * src_diff * sin2alpha )
		/ det_sr ;
	#ifdef DEBUG_ENVELOPE
		cout << "\tsrc_verb_power=" << src_verb_power
			 << " rcv_verb.power=" << rcv_verb.power << endl
			 << "\tdet_sr=" << det_sr
			 << " kappa=" << kappa
			 << " power=" << (10.0*log10(_power)) << endl ;
	#endif
	_power *= exp( kappa ) / sqrt( det_sr ) ;

    // compute the square of the duration of the overlap
    // equation (41) from the paper

    det_sr = det_sr / ( src_prod * rcv_prod ) ;
	_duration = 0.5 * (
			( 1.0 / src_verb.width2 + 1.0 / src_verb.length2 )
			+ ( 1.0 / src_verb.width2 - 1.0 / src_verb.length2 ) * cos2alpha
			+ 2.0 / rcv_verb.width2
			) / det_sr ;

	// combine duration of the overlap with pulse length
	// equation (33) from the paper

	const double factor = cos( rcv_verb.grazing ) / rcv_verb.sound_speed ;
	_duration = 0.5 * sqrt( _pulse_length * _pulse_length
			+ factor * factor * _duration ) ;
	#ifdef DEBUG_ENVELOPE
		cout << "\tcontribution"
			<< " duration=" << _duration
			<< " power=" << (10.0*log10(_power)) << endl ;
	#endif

	// check threshold to avoid calculations for weak signals

	BOOST_FOREACH( double level, _power ) {
		if ( level / _duration > _threshold ) return true ;
	}
	return false ;
}

/**
 * Computes Gaussian time series contribution given delay, duration, and
 * total power.
 */
void envelope_model::compute_time_series(
		double src_verb_time, double rcv_verb_time )
{
	_intensity.clear() ;

	for ( size_t f = 0 ;f < _envelope_freq->size(); ++f ) {

		// compute the peak time and intensity

		const double delay = src_verb_time + rcv_verb_time + _duration - _initial_time;
		const double scale = _power[f] / _duration;

		// compute Gaussian intensity as a function of time

		matrix_row< matrix<double> > intensity( _intensity, f ) ;

		// use uBLAS vector proxies to only compute the portion of the
		// time series within +/- five (5) times the duration
		// speeds up the computation by over a factor of 3

		_level.clear() ;
		size_t first = _travel_time->find_index(delay - 5.0 * _duration);
		size_t last = _travel_time->find_index(delay + 5.0 * _duration) + 1;
		range window(first, last);
		vector_range< seq_vector > time(*_travel_time, window);
		vector_range< vector<double> > level(_level, window);
		level = scale * exp(-0.5 * abs2((time - delay) / _duration)) ;
		intensity = _level ;
	}
}