/* BEGIN software license
 *
 * msXpertSuite - mass spectrometry software suite
 * -----------------------------------------------
 * Copyright(C) 2009,...,2018 Filippo Rusconi
 *
 * http://www.msxpertsuite.org
 *
 * This file is part of the msXpertSuite project.
 *
 * The msXpertSuite project is the successor of the massXpert project. This
 * project now includes various independent modules:
 *
 * - massXpert, model polymer chemistries and simulate mass spectrometric data;
 * - mineXpert, a powerful TIC chromatogram/mass spectrum viewer/miner;
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * END software license
 */


#include <qmath.h>
#include <QVector>
#include <QDebug>


#include <minexpert/nongui/SavGolFilter.hpp>


int savGolfParamsMetaTypeId = qRegisterMetaType<msXpSmineXpert::SavGolParams>(
  "msXpSmineXpert::SavGolParams");


int savGolfFilterMetaTypeId = qRegisterMetaType<msXpSmineXpert::SavGolFilter>(
  "msXpSmineXpert::SavGolFilter");


namespace msXpSmineXpert
{


#define SWAP(a, b) \
  tempr = (a);     \
  (a)   = (b);     \
  (b)   = tempr


//! Construct a SavGolFilter instance
SavGolFilter::SavGolFilter()
{
}


//! Construct a SavGolFilter instance
/*
   The new instance is initialized using the \p other SavGolFilter  object.
   */
SavGolFilter::SavGolFilter(const SavGolFilter &other)
{
  m_savGolParams.nL             = other.m_savGolParams.nL;
  m_savGolParams.nR             = other.m_savGolParams.nR;
  m_savGolParams.m              = other.m_savGolParams.m;
  m_savGolParams.lD             = other.m_savGolParams.lD;
  m_savGolParams.convolveWithNr = other.m_savGolParams.convolveWithNr;

  m_samples = other.m_samples;
}


//! Construct a SavGolFilter instance using the Savitzky-Golay parameters
/*
   \param nL number of data point left of the point being filtered
   \param nR number of data point right of the point being filtered
   \param m order of the polynomial to use in the regression analysis
   \param lD order of the derivative to extract
   \param convolveWithNr set to false
   */
SavGolFilter::SavGolFilter(int nL, int nR, int m, int lD, bool convolveWithNr)
{
  m_savGolParams.nL             = nL;
  m_savGolParams.nR             = nR;
  m_savGolParams.m              = m;
  m_savGolParams.lD             = lD;
  m_savGolParams.convolveWithNr = convolveWithNr;
}


//! Construct a SavGolFilter instance using the SavGolParams struct data
SavGolFilter::SavGolFilter(SavGolParams savGolParams)
{
  initialize(savGolParams);
}


//! Destruct this SavGolFilter instance
SavGolFilter::~SavGolFilter()
{
}


//! Initialize this SavGolFilter instance with the \p other instance
/*
   Only the configuration member data are copied, not the data nor the results.
   */
void
SavGolFilter::initialize(const SavGolFilter &other)
{
  m_savGolParams.nL             = other.m_savGolParams.nL;
  m_savGolParams.nR             = other.m_savGolParams.nR;
  m_savGolParams.m              = other.m_savGolParams.m;
  m_savGolParams.lD             = other.m_savGolParams.lD;
  m_savGolParams.convolveWithNr = other.m_savGolParams.convolveWithNr;

  m_samples = other.m_samples;
}


//! Initialize this SavGolFilter instance with the \p other SavGolParams struct
void
SavGolFilter::initialize(const SavGolParams &other)
{
  m_savGolParams.nL             = other.nL;
  m_savGolParams.nR             = other.nR;
  m_savGolParams.m              = other.m;
  m_savGolParams.lD             = other.lD;
  m_savGolParams.convolveWithNr = other.convolveWithNr;
}


//! Assignment operator
/*
   Only the configuration member data are copied, not the data nor the results.
   */
SavGolFilter &
SavGolFilter::operator=(const SavGolFilter &other)
{
  if(this == &other)
    return *this;

  initialize(other.m_savGolParams);

  m_samples = other.m_samples;

  return *this;
}


//! Initialize the trace data with vectors of double values
/*
 * \param keyData vector of double values containing the keys of the trace
 * \param valData vector of double values containing the values of the trace
 */
int
SavGolFilter::initializeData(const QVector<double> &keyData,
                             const QVector<double> &valData)
{

  // Sanity check, both vector must have the same size.

  if(keyData.size() != valData.size())
    qFatal(
      "Fatal error at %s@%d -- %s(). "
      "Error, both vectors need having the same number of elements."
      "Program aborted.",
      __FILE__,
      __LINE__,
      __FUNCTION__);

  m_samples = keyData.size();

  // qDebug() << __FILE__ << __LINE__ << __FUNCTION__ << "()"
  //<< "number of samples in key data:" << m_samples;

  m_x  = dvector(1, m_samples);
  m_yr = dvector(1, m_samples);
  if(m_savGolParams.convolveWithNr)
    m_yf = dvector(1, 2 * m_samples);
  else
    m_yf = dvector(1, m_samples);

  for(int iter = 0; iter < m_samples; ++iter)
    {
      m_x[iter]  = keyData.at(iter);
      m_yr[iter] = valData.at(iter);

      // qDebug() << __FILE__ << __LINE__ << __FUNCTION__ << "()"
      //<< "initialized " << m_x[iter] << "," << m_yr[iter];
    }

  return m_samples;
}


//! Fill-in \p data with the filtered result data
void
SavGolFilter::filteredData(QVector<double> &data)
{
  for(int iter = 0; iter < m_samples; ++iter)
    {
      data.append(m_yf[iter]);

      // qDebug() << __FILE__ << __LINE__ << __FUNCTION__ << "()"
      //<< "appending index" << iter << "value:" << m_yf[iter];
    }
}


int *
SavGolFilter::ivector(long nl, long nh)
{
  int *v;
  v = (int *)malloc((size_t)((nh - nl + 2) * sizeof(int)));
  if(!v)
    {
      qDebug("Error: Allocation failure.");
      exit(1);
    }
  return v - nl + 1;
}


double *
SavGolFilter::dvector(long nl, long nh)
{
  double *v;
  long k;
  v = (double *)malloc((size_t)((nh - nl + 2) * sizeof(double)));
  if(!v)
    {
      qDebug("Error: Allocation failure.");
      exit(1);
    }
  for(k = nl; k <= nh; k++)
    v[k] = 0.0;
  return v - nl + 1;
}


double **
SavGolFilter::dmatrix(long nrl, long nrh, long ncl, long nch)
{
  long i, nrow = nrh - nrl + 1, ncol = nch - ncl + 1;
  double **m;
  m = (double **)malloc((size_t)((nrow + 1) * sizeof(double *)));
  if(!m)
    {
      qDebug("Allocation failure 1 occurred.");
      exit(1);
    }
  m += 1;
  m -= nrl;
  m[nrl] = (double *)malloc((size_t)((nrow * ncol + 1) * sizeof(double)));
  if(!m[nrl])
    {
      qDebug("Allocation failure 2 occurred.");
      exit(1);
    }
  m[nrl] += 1;
  m[nrl] -= ncl;
  for(i = nrl + 1; i <= nrh; i++)
    m[i] = m[i - 1] + ncol;
  return m;
}


void
SavGolFilter::free_ivector(int *v, long nl, long nh)
{
  free((char *)(v + nl - 1));
}


void
SavGolFilter::free_dvector(double *v, long nl, long nh)
{
  free((char *)(v + nl - 1));
}


void
SavGolFilter::free_dmatrix(double **m, long nrl, long nrh, long ncl, long nch)
{
  free((char *)(m[nrl] + ncl - 1));
  free((char *)(m + nrl - 1));
}


void
SavGolFilter::lubksb(double **a, int n, int *indx, double b[])
{
  int i, ii = 0, ip, j;
  double sum;

  for(i = 1; i <= n; i++)
    {
      ip    = indx[i];
      sum   = b[ip];
      b[ip] = b[i];
      if(ii)
        for(j = ii; j <= i - 1; j++)
          sum -= a[i][j] * b[j];
      else if(sum)
        ii = i;
      b[i] = sum;
    }
  for(i = n; i >= 1; i--)
    {
      sum = b[i];
      for(j = i + 1; j <= n; j++)
        sum -= a[i][j] * b[j];
      b[i] = sum / a[i][i];
    }
}


void
SavGolFilter::ludcmp(double **a, int n, int *indx, double *d)
{
  int i, imax = 0, j, k;
  double big, dum, sum, temp;
  double *vv;

  vv = dvector(1, n);
  *d = 1.0;
  for(i = 1; i <= n; i++)
    {
      big = 0.0;
      for(j = 1; j <= n; j++)
        if((temp = fabs(a[i][j])) > big)
          big = temp;
      if(big == 0.0)
        {
          qDebug("Error: Singular matrix found in routine ludcmp()");
          exit(1);
        }
      vv[i] = 1.0 / big;
    }
  for(j = 1; j <= n; j++)
    {
      for(i = 1; i < j; i++)
        {
          sum = a[i][j];
          for(k = 1; k < i; k++)
            sum -= a[i][k] * a[k][j];
          a[i][j] = sum;
        }
      big = 0.0;
      for(i = j; i <= n; i++)
        {
          sum = a[i][j];
          for(k = 1; k < j; k++)
            sum -= a[i][k] * a[k][j];
          a[i][j] = sum;
          if((dum = vv[i] * fabs(sum)) >= big)
            {
              big  = dum;
              imax = i;
            }
        }
      if(j != imax)
        {
          for(k = 1; k <= n; k++)
            {
              dum        = a[imax][k];
              a[imax][k] = a[j][k];
              a[j][k]    = dum;
            }
          *d       = -(*d);
          vv[imax] = vv[j];
        }
      indx[j] = imax;
      if(a[j][j] == 0.0)
        a[j][j] = std::numeric_limits<double>::epsilon();
      if(j != n)
        {
          dum = 1.0 / (a[j][j]);
          for(i = j + 1; i <= n; i++)
            a[i][j] *= dum;
        }
    }
  free_dvector(vv, 1, n);
}


void
SavGolFilter::four1(double data[], unsigned long nn, int isign)
{
  unsigned long n, mmax, m, j, istep, i;
  double wtemp, wr, wpr, wpi, wi, theta;
  double tempr, tempi;

  n = nn << 1;
  j = 1;
  for(i = 1; i < n; i += 2)
    {
      if(j > i)
        {
          SWAP(data[j], data[i]);
          SWAP(data[j + 1], data[i + 1]);
        }
      m = n >> 1;
      while(m >= 2 && j > m)
        {
          j -= m;
          m >>= 1;
        }
      j += m;
    }
  mmax = 2;
  while(n > mmax)
    {
      istep = mmax << 1;
      theta = isign * (6.28318530717959 / mmax);
      wtemp = sin(0.5 * theta);
      wpr   = -2.0 * wtemp * wtemp;
      wpi   = sin(theta);
      wr    = 1.0;
      wi    = 0.0;
      for(m = 1; m < mmax; m += 2)
        {
          for(i = m; i <= n; i += istep)
            {
              j           = i + mmax;
              tempr       = wr * data[j] - wi * data[j + 1];
              tempi       = wr * data[j + 1] + wi * data[j];
              data[j]     = data[i] - tempr;
              data[j + 1] = data[i + 1] - tempi;
              data[i] += tempr;
              data[i + 1] += tempi;
            }
          wr = (wtemp = wr) * wpr - wi * wpi + wr;
          wi = wi * wpr + wtemp * wpi + wi;
        }
      mmax = istep;
    }
}


void
SavGolFilter::twofft(
  double data1[], double data2[], double fft1[], double fft2[], unsigned long n)
{
  unsigned long nn3, nn2, jj, j;
  double rep, rem, aip, aim;

  nn3 = 1 + (nn2 = 2 + n + n);
  for(j = 1, jj = 2; j <= n; j++, jj += 2)
    {
      fft1[jj - 1] = data1[j];
      fft1[jj]     = data2[j];
    }
  four1(fft1, n, 1);
  fft2[1] = fft1[2];
  fft1[2] = fft2[2] = 0.0;
  for(j = 3; j <= n + 1; j += 2)
    {
      rep           = 0.5 * (fft1[j] + fft1[nn2 - j]);
      rem           = 0.5 * (fft1[j] - fft1[nn2 - j]);
      aip           = 0.5 * (fft1[j + 1] + fft1[nn3 - j]);
      aim           = 0.5 * (fft1[j + 1] - fft1[nn3 - j]);
      fft1[j]       = rep;
      fft1[j + 1]   = aim;
      fft1[nn2 - j] = rep;
      fft1[nn3 - j] = -aim;
      fft2[j]       = aip;
      fft2[j + 1]   = -rem;
      fft2[nn2 - j] = aip;
      fft2[nn3 - j] = rem;
    }
}


void
SavGolFilter::realft(double data[], unsigned long n, int isign)
{
  unsigned long i, i1, i2, i3, i4, np3;
  double c1 = 0.5, c2, h1r, h1i, h2r, h2i;
  double wr, wi, wpr, wpi, wtemp, theta;

  theta = 3.141592653589793 / (double)(n >> 1);
  if(isign == 1)
    {
      c2 = -0.5;
      four1(data, n >> 1, 1);
    }
  else
    {
      c2    = 0.5;
      theta = -theta;
    }
  wtemp = sin(0.5 * theta);
  wpr   = -2.0 * wtemp * wtemp;
  wpi   = sin(theta);
  wr    = 1.0 + wpr;
  wi    = wpi;
  np3   = n + 3;
  for(i = 2; i <= (n >> 2); i++)
    {
      i4       = 1 + (i3 = np3 - (i2 = 1 + (i1 = i + i - 1)));
      h1r      = c1 * (data[i1] + data[i3]);
      h1i      = c1 * (data[i2] - data[i4]);
      h2r      = -c2 * (data[i2] + data[i4]);
      h2i      = c2 * (data[i1] - data[i3]);
      data[i1] = h1r + wr * h2r - wi * h2i;
      data[i2] = h1i + wr * h2i + wi * h2r;
      data[i3] = h1r - wr * h2r + wi * h2i;
      data[i4] = -h1i + wr * h2i + wi * h2r;
      wr       = (wtemp = wr) * wpr - wi * wpi + wr;
      wi       = wi * wpr + wtemp * wpi + wi;
    }
  if(isign == 1)
    {
      data[1] = (h1r = data[1]) + data[2];
      data[2] = h1r - data[2];
    }
  else
    {
      data[1] = c1 * ((h1r = data[1]) + data[2]);
      data[2] = c1 * (h1r - data[2]);
      four1(data, n >> 1, -1);
    }
}


char
SavGolFilter::convlv(double data[],
                     unsigned long n,
                     double respns[],
                     unsigned long m,
                     int isign,
                     double ans[])
{
  unsigned long i, no2;
  double dum, mag2, *fft;

  fft = dvector(1, n << 1);
  for(i = 1; i <= (m - 1) / 2; i++)
    respns[n + 1 - i] = respns[m + 1 - i];
  for(i = (m + 3) / 2; i <= n - (m - 1) / 2; i++)
    respns[i] = 0.0;
  twofft(data, respns, fft, ans, n);
  no2 = n >> 1;
  for(i = 2; i <= n + 2; i += 2)
    {
      if(isign == 1)
        {
          ans[i - 1] =
            (fft[i - 1] * (dum = ans[i - 1]) - fft[i] * ans[i]) / no2;
          ans[i] = (fft[i] * dum + fft[i - 1] * ans[i]) / no2;
        }
      else if(isign == -1)
        {
          if((mag2 = ans[i - 1] * ans[i - 1] + ans[i] * ans[i]) == 0.0)
            {
              qDebug("Attempt of deconvolving at zero response in convlv().");
              return (1);
            }
          ans[i - 1] =
            (fft[i - 1] * (dum = ans[i - 1]) + fft[i] * ans[i]) / mag2 / no2;
          ans[i] = (fft[i] * dum - fft[i - 1] * ans[i]) / mag2 / no2;
        }
      else
        {
          qDebug("No meaning for isign in convlv().");
          return (1);
        }
    }
  ans[2] = ans[n + 1];
  realft(ans, n, -1);
  free_dvector(fft, 1, n << 1);
  return (0);
}


char
SavGolFilter::sgcoeff(double c[], int np, int nl, int nr, int ld, int m)
{
  int imj, ipj, j, k, kk, mm, *indx;
  double d, fac, sum, **a, *b;

  if(np < nl + nr + 1 || nl < 0 || nr < 0 || ld > m || nl + nr < m)
    {
      qDebug("Inconsistent arguments detected in routine sgcoeff.");
      return (1);
    }
  indx = ivector(1, m + 1);
  a    = dmatrix(1, m + 1, 1, m + 1);
  b    = dvector(1, m + 1);
  for(ipj = 0; ipj <= (m << 1); ipj++)
    {
      sum = (ipj ? 0.0 : 1.0);
      for(k = 1; k <= nr; k++)
        sum += pow((double)k, (double)ipj);
      for(k = 1; k <= nl; k++)
        sum += pow((double)-k, (double)ipj);
      mm = (ipj < 2 * m - ipj ? ipj : 2 * m - ipj);
      for(imj = -mm; imj <= mm; imj += 2)
        a[1 + (ipj + imj) / 2][1 + (ipj - imj) / 2] = sum;
    }
  ludcmp(a, m + 1, indx, &d);
  for(j = 1; j <= m + 1; j++)
    b[j] = 0.0;
  b[ld + 1] = 1.0;
  lubksb(a, m + 1, indx, b);
  for(kk = 1; kk <= np; kk++)
    c[kk] = 0.0;
  for(k = -nl; k <= nr; k++)
    {
      sum = b[1];
      fac = 1.0;
      for(mm = 1; mm <= m; mm++)
        sum += b[mm + 1] * (fac *= k);
      kk    = ((np - k) % np) + 1;
      c[kk] = sum;
    }
  free_dvector(b, 1, m + 1);
  free_dmatrix(a, 1, m + 1, 1, m + 1);
  free_ivector(indx, 1, m + 1);
  return (0);
}


//! Perform the Savitzky-Golay filtering process.
/*
   The results are in the \c m_yf C array of double values.
   */
char
SavGolFilter::filter()
{
  int np = m_savGolParams.nL + 1 + m_savGolParams.nR;
  double *c;
  char retval;

#if CONVOLVE_WITH_NR_CONVLV
  c      = dvector(1, m_samples);
  retval = sgcoeff(c,
                   np,
                   m_savGolParams.nL,
                   m_savGolParams.nR,
                   m_savGolParams.lD,
                   m_savGolParams.m);
  if(retval == 0)
    convlv(m_yr, m_samples, c, np, 1, m_yf);
  free_dvector(c, 1, m_samples);
#else
  int j;
  long int k;
  c      = dvector(1, m_savGolParams.nL + m_savGolParams.nR + 1);
  retval = sgcoeff(c,
                   np,
                   m_savGolParams.nL,
                   m_savGolParams.nR,
                   m_savGolParams.lD,
                   m_savGolParams.m);
  if(retval == 0)
    {
      qDebug() << __FILE__ << __LINE__ << __FUNCTION__ << "()"
               << "retval is 0";

      for(k = 1; k <= m_savGolParams.nL; k++)
        {
          for(m_yf[k] = 0.0, j = -m_savGolParams.nL; j <= m_savGolParams.nR;
              j++)
            {
              if(k + j >= 1)
                {
                  m_yf[k] += c[(j >= 0 ? j + 1
                                       : m_savGolParams.nR + m_savGolParams.nL +
                                           2 + j)] *
                             m_yr[k + j];
                }
            }
        }
      for(k = m_savGolParams.nL + 1; k <= m_samples - m_savGolParams.nR; k++)
        {
          for(m_yf[k] = 0.0, j = -m_savGolParams.nL; j <= m_savGolParams.nR;
              j++)
            {
              m_yf[k] +=
                c[(j >= 0 ? j + 1
                          : m_savGolParams.nR + m_savGolParams.nL + 2 + j)] *
                m_yr[k + j];
            }
        }
      for(k = m_samples - m_savGolParams.nR + 1; k <= m_samples; k++)
        {
          for(m_yf[k] = 0.0, j = -m_savGolParams.nL; j <= m_savGolParams.nR;
              j++)
            {
              if(k + j <= m_samples)
                {
                  m_yf[k] += c[(j >= 0 ? j + 1
                                       : m_savGolParams.nR + m_savGolParams.nL +
                                           2 + j)] *
                             m_yr[k + j];
                }
            }
        }
    }

  free_dvector(c, 1, m_savGolParams.nR + m_savGolParams.nL + 1);
#endif

  return (retval);
}


//! Return a string with the textual representation of the configuration data.
QString
SavGolFilter::asText() const
{
  return QString::asprintf(
    "Savitzy-Golay filter parameters:\n"
    "nL: %d ; nR: %d ; m: %d ; lD: %d ; convolveWithNr : %s",
    m_savGolParams.nL,
    m_savGolParams.nR,
    m_savGolParams.m,
    m_savGolParams.lD,
    (m_savGolParams.convolveWithNr ? "true" : "false"));
}


} // namespace msXpSmineXpert
