/* multifit_nlinear/scaling.c
 *
 * Copyright (C) 2015, 2016 Patrick Alken
 *
 * 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, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * This module handles the updating of the scaling matrix D_k in the
 * trust region subproblem:
 *
 * min m_k (dx), || D_k dx || <= Delta_k
 *
 * where m_k(dx) is a model which approximates the cost function
 * F(x_k + dx) near the current iteration point x_k
 *
 * D_k can be updated according to several different strategies.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

static int init_diag_levenberg(const gsl_matrix * J, gsl_vector * diag);
static int update_diag_levenberg(const gsl_matrix * J,
                                 gsl_vector * diag);

static int init_diag_marquardt(const gsl_matrix * J, gsl_vector * diag);
static int update_diag_marquardt (const gsl_matrix * J,
                                  gsl_vector * diag);

static int init_diag_more(const gsl_matrix * J, gsl_vector * diag);
static int update_diag_more(const gsl_matrix * J, gsl_vector * diag);

/* Levenberg scaling, D = I */
static int
init_diag_levenberg(const gsl_matrix * J, gsl_vector * diag)
{
  (void)J; /* avoid unused parameter warning */
  gsl_vector_set_all(diag, 1.0);
  return GSL_SUCCESS;
}

static int
update_diag_levenberg(const gsl_matrix * J, gsl_vector * diag)
{
  (void)J;    /* avoid unused parameter warning */
  (void)diag; /* avoid unused parameter warning */

  /* nothing to do */
  return GSL_SUCCESS;
}

/* initialize diagonal scaling matrix D according to Marquardt method */
static int
init_diag_marquardt(const gsl_matrix * J, gsl_vector * diag)
{
  return update_diag_marquardt(J, diag);
}

/* update diagonal scaling matrix D according to Marquardt method */
static int
update_diag_marquardt (const gsl_matrix * J, gsl_vector * diag)
{
  const size_t p = J->size2;
  size_t j;

  for (j = 0; j < p; j++)
    {
      gsl_vector_const_view v = gsl_matrix_const_column(J, j);
      double norm = gsl_blas_dnrm2(&v.vector);

      if (norm == 0.0)
        norm = 1.0;

      gsl_vector_set(diag, j, norm);
    }

  return GSL_SUCCESS;
}

/* initialize diagonal scaling matrix D according to Eq 6.3 of
 * More, 1978 */
static int
init_diag_more(const gsl_matrix * J, gsl_vector * diag)
{
  int status;

  gsl_vector_set_zero(diag);
  status = update_diag_more(J, diag);

  return status;
}

/* update diagonal scaling matrix D according to Eq. 6.3 of
 * More, 1978 */
static int
update_diag_more (const gsl_matrix * J, gsl_vector * diag)
{
  const size_t p = J->size2;
  size_t j;

  for (j = 0; j < p; j++)
    {
      gsl_vector_const_view v = gsl_matrix_const_column(J, j);
      double norm = gsl_blas_dnrm2(&v.vector);
      double *diagj = gsl_vector_ptr(diag, j);

      if (norm == 0.0)
        norm = 1.0;

      *diagj = GSL_MAX(*diagj, norm);
    }

  return GSL_SUCCESS;
}

static const gsl_multifit_nlinear_scale levenberg_type =
{
  "levenberg",
  init_diag_levenberg,
  update_diag_levenberg
};

static const gsl_multifit_nlinear_scale marquardt_type =
{
  "marquardt",
  init_diag_marquardt,
  update_diag_marquardt
};

static const gsl_multifit_nlinear_scale more_type =
{
  "more",
  init_diag_more,
  update_diag_more
};

const gsl_multifit_nlinear_scale *gsl_multifit_nlinear_scale_levenberg = &levenberg_type;
const gsl_multifit_nlinear_scale *gsl_multifit_nlinear_scale_marquardt = &marquardt_type;
const gsl_multifit_nlinear_scale *gsl_multifit_nlinear_scale_more = &more_type;


/* dht/test_dht.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_dht.h>

/* Test exact small transform.
 */
int
test_dht_exact(void)
{
  int stat = 0;
  double f_in[3] = { 1.0, 2.0, 3.0 };
  double f_out[3];
  gsl_dht * t = gsl_dht_new(3, 1.0, 1.0);
  gsl_dht_apply(t, f_in, f_out);

  /* Check values. */
  if(fabs( f_out[0]-( 0.375254649407520))/0.375254649407520 > 1.0e-14) stat++;
  if(fabs( f_out[1]-(-0.133507872695560))/0.133507872695560 > 1.0e-14) stat++;
  if(fabs( f_out[2]-( 0.044679925143840))/0.044679925143840 > 1.0e-14) stat++;

  /* Check inverse.
   * We have to adjust the normalization
   * so we can use the same precalculated transform.
   */
  gsl_dht_apply(t, f_out, f_in);
  f_in[0] *= 13.323691936314223*13.323691936314223;  /* jzero[1,4]^2 */
  f_in[1] *= 13.323691936314223*13.323691936314223;
  f_in[2] *= 13.323691936314223*13.323691936314223;

  /* The loss of precision on the inverse
   * is a little surprising. However, this
   * thing is quite tricky since the band-limited
   * function represented by the samples {1,2,3}
   * need not be very nice. Like in any spectral
   * application, you really have to have some
   * a-priori knowledge of the underlying function.
   */
  if(fabs( f_in[0]-1.0)/1.0 > 2.0e-05) stat++;
  if(fabs( f_in[1]-2.0)/2.0 > 2.0e-05) stat++;
  if(fabs( f_in[2]-3.0)/3.0 > 2.0e-05) stat++;

  gsl_dht_free(t);

  return stat;
}

/* Test the transform
 * Integrate[x J_0(a x) / (x^2 + 1), {x,0,Inf}] = K_0(a)
 */
int
test_dht_simple(void)
{
  int stat = 0;
  int n;
  double f_in[128];
  double f_out[128];
  gsl_dht * t = gsl_dht_new(128, 0.0, 100.0);

  for(n=0; n<128; n++) {
    const double x = gsl_dht_x_sample(t, n);
    f_in[n] = 1.0/(1.0+x*x);
  }

  gsl_dht_apply(t, f_in, f_out);

  /* This is a difficult transform to calculate this way,
   * since it does not satisfy the boundary condition and
   * it dies quite slowly. So it is not meaningful to
   * compare this to high accuracy. We only check
   * that it seems to be working.
   */
  if(fabs( f_out[0]-4.00)/4.00 > 0.02) stat++;
  if(fabs( f_out[5]-1.84)/1.84 > 0.02) stat++;
  if(fabs(f_out[10]-1.27)/1.27 > 0.02) stat++;
  if(fabs(f_out[35]-0.352)/0.352 > 0.02) stat++;
  if(fabs(f_out[100]-0.0237)/0.0237 > 0.02) stat++;

  gsl_dht_free(t);

  return stat;
}

/* Test the transform
 * Integrate[ x exp(-x) J_1(a x), {x,0,Inf}] = a F(3/2, 2; 2; -a^2)
 */
int
test_dht_exp1(void)
{
  int stat = 0;
  int n;
  double f_in[128];
  double f_out[128];
  gsl_dht * t = gsl_dht_new(128, 1.0, 20.0);

  for(n=0; n<128; n++) {
    const double x = gsl_dht_x_sample(t, n);
    f_in[n] = exp(-x);
  }

  gsl_dht_apply(t, f_in, f_out);

  /* Spot check.
   * Note that the systematic errors in the calculation
   * are quite large, so it is meaningless to compare
   * to a high accuracy.
   */
  if(fabs( f_out[0]-0.181)/0.181 > 0.02) stat++;
  if(fabs( f_out[5]-0.357)/0.357 > 0.02) stat++;
  if(fabs(f_out[10]-0.211)/0.211 > 0.02) stat++;
  if(fabs(f_out[35]-0.0289)/0.0289 > 0.02) stat++;
  if(fabs(f_out[100]-0.00221)/0.00211 > 0.02) stat++;

  gsl_dht_free(t);

  return stat;
}

/* Test the transform
 * Integrate[ x^2 (1-x^2) J_1(a x), {x,0,1}] = 2/a^2 J_3(a)
 */
int
test_dht_poly1(void)
{
  int stat = 0;
  int n;
  double f_in[128];
  double f_out[128];
  gsl_dht * t = gsl_dht_new(128, 1.0, 1.0);

  for(n=0; n<128; n++) {
    const double x = gsl_dht_x_sample(t, n);
    f_in[n] = x * (1.0 - x*x);
  }

  gsl_dht_apply(t, f_in, f_out);

  /* Spot check. This function satisfies the boundary condition,
   * so the accuracy should be ok.
   */
  if(fabs( f_out[0]-0.057274214)/0.057274214    > 1.0e-07) stat++;
  if(fabs( f_out[5]-(-0.000190850))/0.000190850 > 1.0e-05) stat++;
  if(fabs(f_out[10]-0.000024342)/0.000024342    > 1.0e-04) stat++;
  if(fabs(f_out[35]-(-4.04e-07))/4.04e-07       > 1.0e-03) stat++;
  if(fabs(f_out[100]-1.0e-08)/1.0e-08           > 0.25)    stat++;

  gsl_dht_free(t);

  return stat;
}

int main()
{
  gsl_ieee_env_setup ();

  gsl_test( test_dht_exact(),   "Small Exact DHT");
  gsl_test( test_dht_simple(),  "Simple  DHT");
  gsl_test( test_dht_exp1(),    "Exp  J1 DHT");
  gsl_test( test_dht_poly1(),   "Poly J1 DHT");

  exit (gsl_test_summary());
}


#pragma once

#ifdef HAVE_BLAS

// HOTFIX: cblas.h from the atlas-devel package (version 3.10.1-12.el7) on CentOS 7
// does not declare the functions as `extern "C"`, which breaks name mangling.
// Note that nested `extern "C"` is valid and correct:
// https://stackoverflow.com/questions/48099828/what-happens-if-you-nest-extern-c
extern "C" {
#include <cblas.h>
}

inline int blasIgamax( int n, const float *x, int incx )
{
   return cblas_isamax( n, x, incx );
}

inline int blasIgamax( int n, const double *x, int incx )
{
   return cblas_idamax( n, x, incx );
}

/*inline int blasIgamin( int n, const float *x, int incx )
{
   return cblas_isamin( n, x, incx );
}

inline int blasIgamin( int n, const double *x, int incx )
{
   return cblas_idamin( n, x, incx );
}*/

inline float blasGasum( int n, const float *x, int incx )
{
   return cblas_sasum( n, x, incx );
}

inline double blasGasum( int n, const double *x, int incx )
{
   return cblas_dasum( n, x, incx );
}

inline void blasGaxpy( int n, const float alpha,
                       const float *x, int incx,
                       float *y, int incy )
{
   cblas_saxpy( n, alpha, x, incx, y, incy );
}

inline void blasGaxpy( int n, const double alpha,
                       const double* x, int incx,
                       double *y, int incy )
{
   cblas_daxpy( n, alpha, x, incx, y, incy );
}

inline float blasGdot( int n, const float* x, int incx,
                       const float* y, int incy )
{
   return cblas_sdot( n, x, incx, y, incy );
}

inline double blasGdot( int n, const double* x, int incx,
                        const double* y, int incy )
{
   return cblas_ddot( n, x, incx, y, incy );
}

inline float blasGnrm2( int n, const float* x, int incx )
{
   return cblas_snrm2( n, x, incx );
}

inline double blasGnrm2( int n, const double* x, int incx )
{
   return cblas_dnrm2( n, x, incx );
}

inline void blasGscal( int n, const float alpha,
                       float* x, int incx )
{
   cblas_sscal( n, alpha, x, incx );
}

inline void blasGscal( int n, const double alpha,
                       double* x, int incx )
{
   cblas_dscal( n, alpha, x, incx );
}
#endif


/*
Copyright (c) 2015, Patrick Weltevrede
All rights reserved.

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

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

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

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

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

#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "psrsalsa.h"
#include <gsl/gsl_linalg.h>
int linalg_solve_matrix_eq_gauss_jordan(double *matrixa, double *matrixb, int n, int m, verbose_definition verbose)
{
  int *orig_pivot_row, *orig_pivot_column, *pivot_column_done;
  int pivotnr, pivot_col, pivot_row, rowi, coli, i;
  double *ptr1, *ptr2, tmpvalue;
  double largest_element_value;
  int currow;
  orig_pivot_row = malloc(n*sizeof(int));
  orig_pivot_column = malloc(n*sizeof(int));
  pivot_column_done = malloc(n*sizeof(int));
  if(orig_pivot_row == NULL || orig_pivot_column == NULL || pivot_column_done == NULL) {
    printerror(verbose.debug, "ERROR linalg_solve_matrix_eq_gauss_jordan: Memory allocation error.");
    return 1;
  }
  for(coli = 0; coli < n; coli++)
    pivot_column_done[coli] = 0;
  for(pivotnr = 0; pivotnr < n; pivotnr++) {
    largest_element_value = -1.0;
    for(rowi = 0; rowi < n; rowi++) {
      if(pivot_column_done[rowi] != 1) {
 for(coli = 0; coli < n; coli++) {
   if(pivot_column_done[coli] == 0) {
     tmpvalue = matrixa[rowi*n+coli];
     if(fabs(tmpvalue) >= largest_element_value) {
       largest_element_value = fabs(tmpvalue);
       pivot_row=rowi;
       pivot_col=coli;
     }
   }else if(pivot_column_done[coli] > 1) {
     printerror(verbose.debug, "ERROR linalg_solve_matrix_eq_gauss_jordan: The matrix equation is singular, no solution can be determined.");
     return 2;
   }
 }
      }
    }
    if(largest_element_value <= 0.0) {
      printerror(verbose.debug, "ERROR linalg_solve_matrix_eq_gauss_jordan: The matrix equation is singular, no solution can be determined.");
      return 2;
    }
    pivot_column_done[pivot_col] += 1;
    if(pivot_row != pivot_col) {
      ptr1 = matrixa + pivot_row*n;
      ptr2 = matrixa + pivot_col*n;
      for(i = 0; i < n; i++) {
 tmpvalue = *ptr1;
 *ptr1 = *ptr2;
 *ptr2 = tmpvalue;
      }
      ptr1 = matrixb + pivot_row*n;
      ptr2 = matrixb + pivot_col*n;
      for(i = 0; i < n; i++) {
 tmpvalue = *ptr1;
 *ptr1 = *ptr2;
 *ptr2 = tmpvalue;
      }
    }
    orig_pivot_row[pivotnr] = pivot_row;
    orig_pivot_column[pivotnr] = pivot_col;
    matrixa[pivot_col*n+pivot_col]=1.0;
    ptr1 = matrixa + pivot_col*n;
    for(i = 0; i < n; i++) {
      *ptr1 /= largest_element_value;
      ptr1 += 1;
    }
    ptr1 = matrixb + pivot_col*m;
    for(i = 0; i < m; i++) {
      *ptr1 /= largest_element_value;
      ptr1 += 1;
    }
    for(currow = 0; currow < n; currow++) {
      if(currow != pivot_col) {
 i = currow*n+pivot_col;
 tmpvalue = matrixa[i];
 matrixa[i] = 0.0;
 ptr1 = matrixa + n*currow;
 ptr2 = matrixa + n*pivot_col;
 for(i=1; i <= n; i++) {
   *ptr1 -= (*ptr2)*tmpvalue;
   ptr1++;
   ptr2++;
 }
 ptr1 = matrixb + currow*m;
 ptr2 = matrixb + pivot_col*m;
 for(i = 0; i < m; i++) {
   *ptr1 -= (*ptr2)*tmpvalue;
   ptr1++;
   ptr2++;
 }
      }
    }
  }
  for(coli = n-1; coli >= 0; coli--) {
    if(orig_pivot_row[coli] != orig_pivot_column[coli]) {
      for(rowi = 0; rowi <= n; rowi++) {
 tmpvalue = matrixa[rowi*n+orig_pivot_row[coli]];
 matrixa[rowi*n+orig_pivot_row[coli]] = matrixa[rowi*n+orig_pivot_column[coli]];
 matrixa[rowi*n+orig_pivot_column[coli]] = tmpvalue;
      }
    }
  }
  free(pivot_column_done);
  free(orig_pivot_row);
  free(orig_pivot_column);
  return 0;
}