/* movstat/test_Sn.c
 *
 * Copyright (C) 2018 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.
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_test.h>

/* calculate S_n statistic for input vector using slow/naive algorithm */
static int
slow_movSn(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y,
           const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  double *work = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double Sn;

      gsl_sort(window, 1, wsize);
      Sn = gsl_stats_Sn_from_sorted_data(window, 1, wsize, work);
      gsl_vector_set(y, i, Sn);
    }

  free(window);
  free(work);

  return GSL_SUCCESS;
}

static double
func_Sn(const size_t n, double x[], void * params)
{
  double *work = malloc(n * sizeof(double));
  double Sn;

  (void) params;

  gsl_sort(x, 1, n);
  Sn = gsl_stats_Sn_from_sorted_data(x, 1, n, work);

  free(work);

  return Sn;
}

static void
test_Sn_proc(const double tol, const size_t n, const size_t H, const size_t J,
             const gsl_movstat_end_t etype, gsl_rng *rng_p)
{
  gsl_movstat_workspace *w;
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  gsl_vector *z = gsl_vector_alloc(n);
  gsl_movstat_function F;
  char buf[2048];

  F.function = func_Sn;
  F.params = NULL;

  if (H == J)
    w = gsl_movstat_alloc(2*H + 1);
  else
    w = gsl_movstat_alloc2(H, J);

  /* test moving median with random input */
  random_vector(x, rng_p);

  /* y = S_n(x) with slow brute force algorithm */
  slow_movSn(etype, x, y, H, J);

  /* z = S_n(x) */
  gsl_movstat_Sn(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u Sn random", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = S_n(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_Sn(etype, z, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u Sn random in-place", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = S_n(x) with user-defined function */
  gsl_movstat_apply(etype, &F, x, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u Sn user", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
  gsl_movstat_free(w);
}

static void
test_Sn(gsl_rng * rng_p)
{
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 5, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 2, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 2000, 50, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 2000, 0, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 1, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 50, 1, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_Sn_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 5, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 2, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 2000, 50, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 2000, 0, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 1, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 50, 1, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_Sn_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 5, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 1000, 2, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 2000, 50, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 2000, 0, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 1, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Sn_proc(GSL_DBL_EPSILON, 20, 50, 1, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_fit.h>

#include "cosmocalc.h"

double nonlinear_corrfunc_integ_funct(double k, void *p)
{
  double r = ((double*)p)[0];
  double a = ((double*)p)[1];

  return nonlinear_powspec(k,a)*k/r;
}

double nonlinear_corrfunc_exact(double r, double a)
{
  double I0;
  double abserr,p[2];
  gsl_integration_workspace *workspace;
  gsl_function F;
  gsl_integration_qawo_table *wf;
  gsl_integration_workspace *cycle_workspace;

#define WORKSPACE_NUM 70
#define ABSERR 0.0
#define RELERR 1e-3
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  cycle_workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  wf = gsl_integration_qawo_table_alloc(r,1e3-1e-7,GSL_INTEG_SINE,(size_t) WORKSPACE_NUM);

  F.function = &nonlinear_corrfunc_integ_funct;
  p[0] = r;
  p[1] = a;
  F.params = p;
  gsl_integration_qawo(&F,1e-7,ABSERR,RELERR,(size_t) WORKSPACE_NUM,workspace,wf,&I0,&abserr);

  gsl_integration_qawo_table_free(wf);
  gsl_integration_workspace_free(cycle_workspace);
  gsl_integration_workspace_free(workspace);
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM

  return I0/2.0/M_PI/M_PI;
}

double nonlinear_corrfunc(double r, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static double aint;
  static gsl_spline *cosmocalc_nonlinear_corrfunc_spline = NULL;
  static gsl_interp_accel *cosmocalc_nonlinear_corrfunc_acc = NULL;

  double nonlinear_corrfunc_table[COSMOCALC_NONLINEAR_CORRFUNC_TABLE_LENGTH];
  double r_table[COSMOCALC_NONLINEAR_CORRFUNC_TABLE_LENGTH];
  long i;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum || a != aint)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;
      aint = a;

      for(i=0;i<COSMOCALC_NONLINEAR_CORRFUNC_TABLE_LENGTH;++i)
	{
	  r_table[i] = log(CF_R_MAX/CF_R_MIN)/(COSMOCALC_NONLINEAR_CORRFUNC_TABLE_LENGTH-1.0)*((double) i) + log(CF_R_MIN);
	  nonlinear_corrfunc_table[i] = nonlinear_corrfunc_exact(exp(r_table[i]),aint);
	}

      //init the spline and accelerators
      if(cosmocalc_nonlinear_corrfunc_spline != NULL)
	gsl_spline_free(cosmocalc_nonlinear_corrfunc_spline);
      cosmocalc_nonlinear_corrfunc_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_NONLINEAR_CORRFUNC_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_nonlinear_corrfunc_spline,r_table,nonlinear_corrfunc_table,(size_t) (COSMOCALC_NONLINEAR_CORRFUNC_TABLE_LENGTH));
      if(cosmocalc_nonlinear_corrfunc_acc != NULL)
	gsl_interp_accel_reset(cosmocalc_nonlinear_corrfunc_acc);
      else
	cosmocalc_nonlinear_corrfunc_acc = gsl_interp_accel_alloc();
    }

  if(r < CF_R_MIN)
    return 0.0;
  else if(r < CF_R_MAX)
    return gsl_spline_eval(cosmocalc_nonlinear_corrfunc_spline,log(r),cosmocalc_nonlinear_corrfunc_acc);
  else
    return 0.0;
}


#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include <gsl/gsl_math.h>

#include "gaussianInt2.h"
#include "blit3.h"

#ifndef verbose
#define verbose 1
#endif

double rand_range(double A, double B)
  // A random number between A and B
{
  if(A==B)
    return A;
  if(B>A)
    return A + (double) rand() / RAND_MAX * (B-A);
  //if(A>B)
  return B + (double) rand() / RAND_MAX * (A-B);
}

int blit3g(double * T, uint32_t Tm, uint32_t Tn, uint32_t Tp,
    double * D, size_t nD, photonw pw, int one_indexing)
  // T:  target volume
  // nD: number of dots
  // D:  list of dots (x,y,z, nphot, sigmax, sigmay, sigmaz)
{

  const int nF = 7; // stride for D

  double sigma_max = -1;
  for(size_t kk = 0; kk < nD; kk++)
    {
      for(int ll = 0; ll < 3; ll++)
      {
      if( D[nF*kk+4 + ll] > sigma_max)
        sigma_max = D[nF*kk+4 + ll];
      }
    }

  const size_t w = ceil(sigma_max)*3*2 + 1;
  const int hw = (w-1)/2;
  // allocate temporary space for the Gaussian
  double * G = malloc(w*w*w*sizeof(double));

  double sigma[] = {1.6,1.6,2.1};
  double mu[] = {0,0,0};
  double nPhot;

  for(uint32_t pp = 0; pp<nD; pp++)  {

    // Round the location
    int64_t Pm =(int64_t) (D[nF*pp+0]-.5);
    int64_t Pn =(int64_t) (D[nF*pp+1]-.5);
    int64_t Pp =(int64_t) (D[nF*pp+2]-.5);

    //    printf("%lu %lu %lu\n", Pm,Pn,Pp);

    // Get the offset from the centre of the pixel
    mu[0] = -roundf(D[nF*pp+0])+D[nF*pp+0];
    mu[1] = -roundf(D[nF*pp+1])+D[nF*pp+1];
    mu[2] = -roundf(D[nF*pp+2])+D[nF*pp+2];

    //    printf("%f %f %f\n", mu[0], mu[1], mu[1]);

    //   printf("mu: %f %f %f\n", mu[0], mu[1], mu[2]);
    sigma[0] = D[nF*pp+4];
    sigma[1] = D[nF*pp+5];
    sigma[2] = D[nF*pp+6];

    nPhot = D[nF*pp+3];
    // Create the kernel
    if(!gaussianInt3(G, mu,  sigma, w))
    {
      printf("Failed to allocate space for Gaussian kernel\n");
      return 1;
    }

    if(pw == xyz_volume)
      for(size_t tt =0; tt<w*w*w; tt++)
        G[tt] = G[tt]*nPhot;

    if(pw == xy_plane)
    {
      double wgt = 0;
      for(size_t tt =0; tt<w*w; tt++)
        wgt += G[tt+w*w*(w-1)/2];

      for(size_t tt =0; tt<w*w*w; tt++)
        G[tt] = G[tt]*nPhot/wgt;
    }

    if(pw == mid_point)
    {
      const double wgt = G[(w*w*w-1)/2];
      for(size_t tt =0; tt<w*w*w; tt++)
        G[tt] = G[tt]*nPhot/wgt;
    }

    // blit it
    if(one_indexing){
      blit3(T, Tm, Tn, Tp,
          G, w, w, w,
          Pm-hw, Pn-hw, Pp-hw,
          0);
    } else {
blit3(T, Tm, Tn, Tp,
          G, w, w, w,
          Pm-hw+1, Pn-hw+1, Pp-hw+1,
          0);
    }

  }
  free(G);
  return 0;
}

void blit3(double * T, uint32_t Tm, uint32_t Tn, uint32_t Tp,
    double * S, uint32_t Sm, uint32_t Sn, uint32_t Sp,
    int64_t Pm, int64_t Pn, int64_t Pp,
    int8_t anchor)
  // T: target image
{
  // anchor, 0=corner, 1=center of S
  assert(anchor == 0);
  // Which ranges in T to be visited
  // in [Pma, Pmb]
  uint32_t Tma = GSL_MAX(0, Pm);
  uint32_t Tmb = GSL_MIN(Tm-1, Pm+Sm-1);
  uint32_t Tna = GSL_MAX(0, Pn);
  uint32_t Tnb = GSL_MIN(Tn-1, Pn+Sn-1);
  uint32_t Tpa = GSL_MAX(0, Pp);
  uint32_t Tpb = GSL_MIN(Tp-1, Pp+Sp-1);

  uint32_t Sma = 0;
  if(Pm<0)
    Sma = -Pm;

  uint32_t Sna = 0;
  if(Pn<0)
    Sna = -Pn;

  uint32_t Spa = 0;
  if(Pp<0)
    Spa = -Pp;

#if verbose > 0

  uint32_t Smb = GSL_MIN(Sm-1, Tm-Pm-1);
  uint32_t Snb = GSL_MIN(Sn-1, Tn-Pn-1);
  uint32_t Spb = GSL_MIN(((int64_t) Sp)-1, Tp-Pp-1);
  printf("Size of T: %u x %u x %u\n", Tm, Tn, Tp);
  printf("Size of S: %u x %u x %u\n", Sm, Sn, Sp);
  printf("P: %ld %ld %ld\n", Pm, Pn, Pp);
  printf("Will visit:\n");
  printf("T: [%u %u]x[%u %u]x[%u %u] (%u)\n", Tma, Tmb, Tna, Tnb, Tpa, Tpb, (Tmb-Tma+1)*(Tnb-Tna+1)*(Tpb-Tpa+1));
  printf("S: [%u %u]x[%u %u]x[%u %u] (%u)\n", Sma, Smb, Sna, Snb, Spa, Spb, (Smb-Sma+1)*(Snb-Sna+1)*(Spb-Spa+1));
#endif

  uint32_t ppos, npos; // For pre-caluclation of multiplications

  uint32_t smm=Sma, snn=Sna, spp=Spa;

  for(uint32_t pp = Tpa; pp<=Tpb; pp++, spp++) {
    snn = Sna;

    ppos = pp*Tm*Tn;
    for(uint32_t nn = Tna; nn<=Tnb; nn++, snn++) {
      npos = nn*Tm;
      smm = Sma;

      for(uint32_t mm = Tma; mm<=Tmb; mm++, smm++) {
#if verbose > 1
        printf("T(%u %u %u) := S(%u %u %u)\n", mm, nn, pp, smm, snn, spp);
#endif
        T[mm + npos + ppos] += S[smm + snn*Sm + spp*Sm*Sn];

      }
    }
  }

}

#ifdef standalone
int unit_tests()
{
  printf("Testing \n");
  uint32_t Tm = 10;
  uint32_t Tn = 11;
  uint32_t Tp = 12;

  double * T = malloc(Tm*Tn*Tp*sizeof(double));
  for(size_t kk = 0; kk<Tm*Tn*Tp; kk++) {
    T[kk] = 0;
  }

  uint32_t Sm = 13;
  uint32_t Sn = 15;
  uint32_t Sp = 17;

  double * S = malloc(Sm*Sn*Sp*sizeof(double));

  for(size_t kk = 0; kk<Sm*Sn*Sp; kk++) {
    S[kk] = 1;
  }

  int64_t Pm = -7;
  int64_t Pn = -8;
  int64_t Pp = -9;

  blit3(T, Tm, Tn, Tp,   S, Sm, Sn, Sp,  Pm, Pn, Pp,  0);

  if(Pm>-1 && Pn>-1 && Pp>-1)
    if(Pm<Tm && Pn<Tn && Pp<Tp)
      printf("T(%ld,%ld,%ld) = %f\n", Pm, Pn, Pp, T[Pm + Pn*Tm + Pp*Tm*Tn]);

  free(S);
  free(T);

printf("--> blit3g\n");
size_t Gm = 11;
size_t Gn = 11;
size_t Gp = 11;
double * G = calloc(Gm*Gn*Gp,sizeof(double));
double * Dot = calloc(7,sizeof(double));
Dot[0] = 7; Dot[1] = 7; Dot[2] = 7;
Dot[3] = 100;
Dot[4] = 1; Dot[5]=1; Dot[6]=1;
blit3g(G, Gm, Gn, Gp, Dot, 1, mid_point, 0);

double maxg = 0;
for(size_t kk=0; kk<Gm*Gn*Gp; kk++)
  maxg = GSL_MAX(maxg, G[kk]);
printf("max(G): %f\n", maxg);
double midg = G[7+7*Gm+7*Gm*Gn];
printf("G(7,7,7)=%f\n", midg);
assert(midg == maxg);

free(G);
free(Dot);

  return 0;
}

int main(int argc, char ** argv)
{
  printf("%s\n", argv[0]);
  if(argc == 1)
    return unit_tests();
}
#endif


//
// Created by root on 14.03.20.
//

#ifndef NETWORK_NETWORK_H
#define NETWORK_NETWORK_H

#include <memory>
#include <vector>
#include <functional>
#include <cblas.h>
#include <iostream>
#include <random>
#include "cmake-build-debug/data.pb.h"
#include "Utils.h"

extern std::mt19937 generator;

struct Layer {
    std::unique_ptr<float[]> weights;
    std::unique_ptr<float[]> biases;
    std::unique_ptr<float[]> pre_activ;
    std::unique_ptr<float[]> post_activ;
    std::unique_ptr<float[]> deltas;
    size_t num_rows;
    size_t num_cols;
    std::function<void()> activation;
    std::function<void(float *, float *)> derivative;

    void forward_pass(float *input);

};

class Network {

private:

    std::vector<Layer> layers;
    size_t input_size;

    //looking over backprop implementation
    //Something is not working correctly
    template<typename Loss>
    void back_propagate(float *act_output) {
        {
            Layer &current = get_output_layer();
            const size_t out_neurons = current.num_rows;

            std::unique_ptr<float[]> loss_deriv = std::make_unique<float[]>(out_neurons);
            //Computing the derivative of the loss function with respect to the net_output
            Loss::apply_deriv(current.post_activ.get(), current.post_activ.get() + out_neurons,
                              act_output, loss_deriv.get());

            current.derivative(current.pre_activ.get(), current.deltas.get());
            std::transform(current.deltas.get(), current.deltas.get() + out_neurons, loss_deriv.get(),
                           current.deltas.get(), std::multiplies<float>{});

        }

        //stuff for all the other layers
        for (int l = layers.size() - 2; l >= 0; --l) {
            Layer &last = layers[l + 1];
            Layer &current = layers[l];

            std::unique_ptr<float[]> deriv_out = std::make_unique<float[]>(current.num_rows);
            //
            current.derivative(current.pre_activ.get(), deriv_out.get());

            //now we can compute the deltas of all the other layers
            cblas_sgemv(CblasRowMajor, CblasTrans, last.num_rows, last.num_cols, 1.0f, last.weights.get(),
                        last.num_cols, last.deltas.get(),
                        1, 0.0f, current.deltas.get(), 1);

            std::transform(current.deltas.get(), current.deltas.get() + current.num_rows, deriv_out.get(),
                           current.deltas.get(), std::multiplies<float>{});

        }
    }

public:

    Network(size_t input) : input_size(input) {}

    template<typename Activ>
    void add_layer(size_t num_hidden) {
        Layer layer;
        if (layers.empty()) {
            layer.num_cols = input_size;
        } else {
            const Layer &last = layers.back();
            layer.num_cols = last.num_rows;
        }
        layers.emplace_back(std::move(layer));
        Layer &current = layers.back();

        current.num_rows = num_hidden;
        current.weights = std::make_unique<float[]>(current.num_rows * current.num_cols);
        current.biases = std::make_unique<float[]>(current.num_rows);
        current.pre_activ = std::make_unique<float[]>(current.num_rows);
        current.post_activ = std::make_unique<float[]>(current.num_rows);
        current.deltas = std::make_unique<float[]>(current.num_rows);
        //init everything but weights to zero
        std::fill(current.deltas.get(), current.deltas.get() + current.num_rows, 0.0f);
        std::fill(current.biases.get(), current.biases.get() + current.num_rows, 0.0f);

        const int rows = current.num_rows;

        float *in = current.pre_activ.get();
        float *out = current.post_activ.get();
        current.activation = [rows, in, out]() {
            Activ::apply(in, in + rows, out);
        };

        current.derivative = [rows](float *in, float *out) {
            Activ::apply_deriv(in, in + rows, out);
        };
    }

    void init_weights();

    template<typename Loss>
    void train(Train::Data &data, float lr, size_t epoch) {
        std::unique_ptr<float[]> input = std::make_unique<float[]>(input_size);
        std::unique_ptr<float[]> output = std::make_unique<float[]>(get_output_layer().num_rows);
        for (auto i = 0; i < epoch; ++i) {
            std::shuffle(data.mutable_data()->begin(), data.mutable_data()->end(), generator);

            for (auto k = 0; k < data.data_size(); ++k) {
                const Train::Point &point = data.data(k);
                for (auto p = 0; p < input_size; ++p) {
                    input[p] = point.input(p);
                }
                for (auto p = 0; p < get_output_layer().num_rows; ++p) {
                    output[p] = point.output(p);
                }
                forward_pass(input.get());

                back_propagate<Loss>(output.get());

                for (Layer &layer : layers) {
                    for (auto x = 0; x < layer.num_rows; ++x) {
                        layer.biases[x] -= lr * layer.deltas[x];
                    }
                }
                for (auto x = 0; x < layers.front().num_rows; ++x) {
                    for (auto y = 0; y < input_size; ++y) {

                        layers.front().weights[x * layers.front().num_cols + y] -=
                                lr * layers.front().deltas[x] * input[y];
                    }
                }

                for (auto l = 1; l < layers.size(); ++l) {
                    Layer &layer = layers[l];
                    Layer &previous = layers[l - 1];

                    for (auto x = 0; x < layer.num_rows; ++x) {
                        for (auto y = 0; y < layer.num_cols; ++y) {
                            layer.weights[x * layer.num_cols + y] -= lr * layer.deltas[x] * previous.post_activ[y];
                        }
                    }
                }

            }
        }
    }

    void forward_pass(float *input);

    Layer &get_output_layer();

    const Layer &get_output_layer() const;

    void train(Train::Data &data, float lr, size_t epoch);
};

#endif //NETWORK_NETWORK_H


/* eigen/test.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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_complex_math.h>
#include "gsl_eigen.h"

gsl_matrix *
create_hilbert_matrix(int size)
{
  int i, j;
  gsl_matrix * m = gsl_matrix_alloc(size, size);
  for(i=0; i<size; i++) {
    for(j=0; j<size; j++) {
      gsl_matrix_set(m, i, j, 1.0/(i+j+1.0));
    }
  }
  return m;
}

gsl_matrix *
create_random_symm_matrix(int size)
{
  int i, j;
  unsigned long k = 1;
  gsl_matrix * m = gsl_matrix_alloc(size, size);
  for(i=0; i<size; i++) {
    for(j=i; j<size; j++) {
      double x;
      k = (69069 * k + 1) & 0xffffffffUL;
      x = k / 4294967296.0;
      gsl_matrix_set(m, i, j, x);
      gsl_matrix_set(m, j, i, x);
    }
  }
  return m;
}

gsl_matrix_complex *
create_random_herm_matrix(int size)
{
  int i, j;
  unsigned long k = 1;
  gsl_matrix_complex * m = gsl_matrix_complex_alloc(size, size);
  for(i=0; i<size; i++) {
    for(j=i; j<size; j++) {
      gsl_complex z;
      k = (69069 * k + 1) & 0xffffffffUL;
      GSL_REAL(z) = k / 4294967296.0;
      k = (69069 * k + 1) & 0xffffffffUL;
      GSL_IMAG(z) = (i == j) ? 0 : k / 4294967296.0;
      gsl_matrix_complex_set(m, i, j, z);
      gsl_matrix_complex_set(m, j, i, gsl_complex_conjugate(z));
    }
  }
  return m;
}

void
test_eigen_results (size_t N, const gsl_matrix * m, const gsl_vector * eval,
                    const gsl_matrix * evec, const char * desc,
                    const char * desc2)
{
  size_t i,j;

  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * y = gsl_vector_alloc(N);

  /* check eigenvalues */

  for (i = 0; i < N; i++)
    {
      double ei = gsl_vector_get (eval, i);
      gsl_vector_const_view vi = gsl_matrix_const_column(evec, i);
      gsl_vector_memcpy(x, &vi.vector);
      /* compute y = m x (should = lambda v) */
      gsl_blas_dgemv (CblasNoTrans, 1.0, m, x, 0.0, y);
      for (j = 0; j < N; j++)
        {
          double xj = gsl_vector_get (x, j);
          double yj = gsl_vector_get (y, j);
          gsl_test_rel(yj, ei * xj,  1e8 * GSL_DBL_EPSILON,
                       "%s, eigenvalue(%d,%d), %s", desc, i, j, desc2);
        }
    }

  /* check eigenvectors are orthonormal */

  for (i = 0; i < N; i++)
    {
      gsl_vector_const_view vi = gsl_matrix_const_column(evec, i);
      double nrm_v = gsl_blas_dnrm2(&vi.vector);
      gsl_test_rel (nrm_v, 1.0, N * GSL_DBL_EPSILON, "%s, normalized(%d), %s",
                    desc, i, desc2);
    }

  for (i = 0; i < N; i++)
    {
      gsl_vector_const_view vi = gsl_matrix_const_column(evec, i);
      for (j = i + 1; j < N; j++)
        {
          gsl_vector_const_view vj = gsl_matrix_const_column(evec, j);
          double vivj;
          gsl_blas_ddot (&vi.vector, &vj.vector, &vivj);
          gsl_test_abs (vivj, 0.0, N * GSL_DBL_EPSILON,
                        "%s, orthogonal(%d,%d), %s", desc, i, j, desc2);
        }
    }

  gsl_vector_free(x);
  gsl_vector_free(y);
}

void
test_eigenvalues (size_t N, const gsl_vector *eval, const gsl_vector * eval2,
                  const char * desc, const char * desc2)
{
  size_t i;
  for (i = 0; i < N; i++)
    {
      double ei = gsl_vector_get (eval, i);
      double e2i = gsl_vector_get (eval2, i);
      gsl_test_rel(ei, e2i, GSL_DBL_EPSILON, "%s, direct eigenvalue(%d), %s",
                   desc, i, desc2);
    }
}

void
test_eigen_complex_results (size_t N, const gsl_matrix_complex * m,
                            const gsl_vector * eval,
                            const gsl_matrix_complex * evec,
                            const char * desc,
                            const char * desc2)
{
  size_t i,j;

  gsl_vector_complex * x = gsl_vector_complex_alloc(N);
  gsl_vector_complex * y = gsl_vector_complex_alloc(N);

  /* check eigenvalues */

  for (i = 0; i < N; i++)
    {
      double ei = gsl_vector_get (eval, i);
      gsl_vector_complex_const_view vi = gsl_matrix_complex_const_column(evec, i);
      gsl_vector_complex_memcpy(x, &vi.vector);
      /* compute y = m x (should = lambda v) */
      gsl_blas_zgemv (CblasNoTrans, GSL_COMPLEX_ONE, m, x,
                      GSL_COMPLEX_ZERO, y);
      for (j = 0; j < N; j++)
        {
          gsl_complex xj = gsl_vector_complex_get (x, j);
          gsl_complex yj = gsl_vector_complex_get (y, j);
          gsl_test_rel(GSL_REAL(yj), ei * GSL_REAL(xj), 1e8*GSL_DBL_EPSILON,
                       "%s, eigenvalue(%d,%d), real, %s", desc, i, j, desc2);
          gsl_test_rel(GSL_IMAG(yj), ei * GSL_IMAG(xj), 1e8*GSL_DBL_EPSILON,
                       "%s, eigenvalue(%d,%d), imag, %s", desc, i, j, desc2);
        }
    }

  /* check eigenvectors are orthonormal */

  for (i = 0; i < N; i++)
    {
      gsl_vector_complex_const_view vi = gsl_matrix_complex_const_column(evec, i);
      double nrm_v = gsl_blas_dznrm2(&vi.vector);
      gsl_test_rel (nrm_v, 1.0, N * GSL_DBL_EPSILON, "%s, normalized(%d), %s",
                    desc, i, desc2);
    }

  for (i = 0; i < N; i++)
    {
      gsl_vector_complex_const_view vi = gsl_matrix_complex_const_column(evec, i);
      for (j = i + 1; j < N; j++)
        {
          gsl_vector_complex_const_view vj
            = gsl_matrix_complex_const_column(evec, j);
          gsl_complex vivj;
          gsl_blas_zdotc (&vi.vector, &vj.vector, &vivj);
          gsl_test_abs (gsl_complex_abs(vivj), 0.0, N * GSL_DBL_EPSILON,
                        "%s, orthogonal(%d,%d), %s", desc, i, j, desc2);
        }
    }

  gsl_vector_complex_free(x);
  gsl_vector_complex_free(y);
}

void
test_eigen_symm(const char * desc, const gsl_matrix * m)
{
  size_t N = m->size1;

  gsl_matrix * A = gsl_matrix_alloc(N, N);
  gsl_matrix * evec = gsl_matrix_alloc(N, N);
  gsl_vector * eval = gsl_vector_alloc(N);
  gsl_vector * eval2 = gsl_vector_alloc(N);

  gsl_eigen_symm_workspace * w1 = gsl_eigen_symm_alloc (N);
  gsl_eigen_symmv_workspace * w2 = gsl_eigen_symmv_alloc (N);

  gsl_matrix_memcpy(A, m);
  gsl_eigen_symmv(A, eval, evec, w2);
  test_eigen_results (N, m, eval, evec, desc, "unsorted");

  gsl_matrix_memcpy(A, m);
  gsl_eigen_symm(A, eval2, w1);
  test_eigenvalues (N, eval, eval2, desc, "unsorted");

  gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_VAL_ASC);
  test_eigen_results (N, m, eval, evec, desc, "val/asc");

  gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_VAL_DESC);
  test_eigen_results (N, m, eval, evec, desc, "val/desc");

  gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
  test_eigen_results (N, m, eval, evec, desc, "abs/asc");

  gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_VAL_DESC);
  test_eigen_results (N, m, eval, evec, desc, "abs/desc");

  gsl_eigen_symm_free (w1);
  gsl_eigen_symmv_free (w2);

  gsl_matrix_free(A);
  gsl_matrix_free(evec);
  gsl_vector_free(eval);
  gsl_vector_free(eval2);
}

void
test_eigen_herm(const char * desc, const gsl_matrix_complex * m)
{
  size_t N = m->size1;

  gsl_matrix_complex * A = gsl_matrix_complex_alloc(N, N);
  gsl_matrix_complex * evec = gsl_matrix_complex_alloc(N, N);
  gsl_vector * eval = gsl_vector_alloc(N);
  gsl_vector * eval2 = gsl_vector_alloc(N);

  gsl_eigen_herm_workspace * w1 = gsl_eigen_herm_alloc (N);
  gsl_eigen_hermv_workspace * w2 = gsl_eigen_hermv_alloc (N);

  gsl_matrix_complex_memcpy(A, m);
  gsl_eigen_hermv(A, eval, evec, w2);
  test_eigen_complex_results (N, m, eval, evec, desc, "unsorted");

  gsl_matrix_complex_memcpy(A, m);
  gsl_eigen_herm(A, eval2, w1);
  test_eigenvalues (N, eval, eval2, desc, "unsorted");

  gsl_eigen_hermv_sort (eval, evec, GSL_EIGEN_SORT_VAL_ASC);
  test_eigen_complex_results (N, m, eval, evec, desc, "val/asc");

  gsl_eigen_hermv_sort (eval, evec, GSL_EIGEN_SORT_VAL_DESC);
  test_eigen_complex_results (N, m, eval, evec, desc, "val/desc");

  gsl_eigen_hermv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
  test_eigen_complex_results (N, m, eval, evec, desc, "abs/asc");

  gsl_eigen_hermv_sort (eval, evec, GSL_EIGEN_SORT_VAL_DESC);
  test_eigen_complex_results (N, m, eval, evec, desc, "abs/desc");

  gsl_eigen_herm_free (w1);
  gsl_eigen_hermv_free (w2);

  gsl_matrix_complex_free(A);
  gsl_matrix_complex_free(evec);
  gsl_vector_free(eval);
  gsl_vector_free(eval2);
}

void
test_eigen_jacobi(const char * desc, const gsl_matrix * m)
{
  size_t N = m->size1;
  unsigned int nrot;

  gsl_matrix * A = gsl_matrix_alloc(N, N);
  gsl_matrix * evec = gsl_matrix_alloc(N, N);
  gsl_vector * eval = gsl_vector_alloc(N);

  gsl_matrix_memcpy(A, m);
  gsl_eigen_jacobi(A, eval, evec, 1000, &nrot);
  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_ASC);

  test_eigen_results (N, m, eval, evec, desc, "");

  gsl_matrix_free(A);
  gsl_matrix_free(evec);
  gsl_vector_free(eval);
}

int test_invert_jacobi(void)
{
  int s = 0;
  int i, j;
  gsl_matrix * hminv = gsl_matrix_alloc(10, 10);
  gsl_matrix * id    = gsl_matrix_alloc(10, 10);

  /* 10x10 Hilbert matrix */
  gsl_matrix * hm = create_hilbert_matrix(10);
  gsl_eigen_invert_jacobi(hm, hminv, 1000);

  /* gsl_linalg_matmult(hm, hminv, id); */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, hm, hminv, 0.0, id);

  for(i=0; i<10; i++) {
    for(j=0; j<10; j++) {
      double delta_ij = ( i == j ? 1.0 : 0.0 );
      double id_ij    = gsl_matrix_get(id, i, j);
      int rs = ( fabs(id_ij - delta_ij) > 5.0e-3 );
      s += rs;
    }
  }

  gsl_matrix_free(hm);
  gsl_matrix_free(hminv);
  gsl_matrix_free(id);

  return s;
}

int main()
{
  gsl_matrix *rs10 = create_random_symm_matrix (10);
  gsl_matrix_complex *rh10 = create_random_herm_matrix (10);

  double r[] =  { 0,  0, -1,  0,
                  0,  1,  0,  1,
                 -1,  0,  0,  0,
                  0,  1,  0,  0 };

  double c[] =  { 0,0,  0,0, -1,0,  0,0,
                  0,0,  1,0,  0,0,  1,0,
                 -1,0,  0,0,  0,0,  0,0,
                  0,0,  1,0,  0,0,  0,0 };

  gsl_matrix_view s4 = gsl_matrix_view_array (r, 4, 4);
  gsl_matrix_complex_view h4 = gsl_matrix_complex_view_array (c, 4, 4);

  gsl_ieee_env_setup ();

  test_eigen_symm("symm(4)", &s4.matrix);
  test_eigen_herm("herm(4)", &h4.matrix);

  test_eigen_symm("symm(10)", rs10);
  test_eigen_herm("herm(10)", rh10);

  /* gsl_matrix *h5 = create_hilbert_matrix (5); */
  /* test_eigen_jacobi("hilbert(5)", h5); */
  /* test_invert_jacobi(); */
  /* gsl_matrix_free (h5); */

  gsl_matrix_complex_free (rh10);
  gsl_matrix_free (rs10);

  exit (gsl_test_summary());
}


#ifndef __calcAinvb__
#define __calcAinvb__
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_multiroots.h>
#define GSL 2
int GSLtag;
void least_gsl_error_handler( const char * reason,
			      const char * file,
			      int line,
			      int gsl_errno)
{
  fprintf(stderr,"%s:%d: %s (gsl_error %d)\n",  file, line, reason,gsl_errno);
  GSLtag=1;
}

#define ZERO (1e-10)
double VarY;
int calcAinvb1(double *x_data, double *A_data, double *b_data, int nx, int ndata)
{
  /*
    b= A x  --> x= Ainv b
     b  in R^{ndata x 1}
     A  in R^{ndata x nx}   (ndata>nx)
     x  in R^{nx    x 1}
   */
  int i;

  if(ndata<=0){
    x_data[0]=1;
    for(i=1;i<nx;i++) x_data[i]=1./nx;
  }
  else{
    gsl_matrix *A =gsl_matrix_alloc(ndata,nx);
    gsl_vector *b =gsl_vector_alloc(ndata);
    gsl_matrix *V = gsl_matrix_alloc (nx,nx);
    gsl_vector *S = gsl_vector_alloc (nx);
    gsl_vector *work = gsl_vector_alloc(nx);
    gsl_vector *x = gsl_vector_alloc(nx);
    A->data=A_data;
    b->data=b_data;
    x->data=x_data;
    gsl_set_error_handler( &least_gsl_error_handler );GSLtag=0;

#if GSL == 1
    gsl_linalg_SV_decomp_jacobi (A, V, S); //higer accuracy than Golub-Reinsh
#elif GSL == 2
    gsl_linalg_SV_decomp (A, V, S, work); //Golub-Reinsh
#elif GSL == 3
    {//faster
      gsl_matrix *X;
      X = gsl_matrix_alloc (nx,nx);
      gsl_linalg_SV_decomp_mod (A, X, V, S, work);
      gsl_matrix_free(X);
    }
#endif
    VarY=0;
    if(GSLtag==1){
      fprintf(stderr,"\nError:EigenValue=");
      for(i=0;i<nx;i++) fprintf(stderr,"%e ",S->data[i]); fprintf(stderr,"\n");
      for(i=0;i<nx;i++) x_data[i]=1./nx;
    }
    else{
      int i,j,l;
      double *Si=(double*)malloc(sizeof(double)*nx);
      double *Ai=(double*)malloc(sizeof(double)*nx*ndata);
      for(i=0;i<nx;i++) if(S->data[i]>1e-10) Si[i]=1./S->data[i]; else Si[i]=0;
      for(i=0;i<nx;i++){
	for(j=0;j<ndata;j++){
	  double *Ai_ij=&Ai[i*ndata+j];
	  Ai_ij[0]=0;
	  for(l=0;l<nx;l++) Ai_ij[0]+=V->data[i*nx+l]*Si[l]*A->data[j*nx+l];
	}
      }
      //      for(i=0;i<nx;i++) x->data[i]=0;
      x->data[0]=0;for(j=0;j<nx;j++) x->data[0]+=Ai[j]*b->data[j];
//      int check=0;
//      if(check){
//	gsl_linalg_SV_solve(A,V,S,b,x);
//	double xi;
//	for(i=0;i<nx;i++){
//	  xi=0;
//	  for(j=0;j<nx;j++) xi+= Ai[i*ndata+j]*b->data[j];
//	  fprintf(stderr,"x[%d]=%g =?= %g\n",i,xi,x->data[i]);
//	}
//	fprintf(stderr,"p:");
//	for(i=0;i<nx;i++){
//	  fprintf(stderr,"\n p(i%d,*):",i);
//	  for(j=0;j<nx;j++){
//	    fprintf(stderr,"%+.3f ",Ai[i*nx+j]);
//	  }
//	}
//	fprintf(stderr,"\n:");
//	fprintf(stderr,"check\n");
//      }
      free(Si);
      free(Ai);
    }
    gsl_vector_free(S);
    gsl_vector_free(work);
    gsl_vector_free(x);
    gsl_matrix_free(V);
    gsl_matrix_free(A);
    gsl_vector_free(b);
  }
  return 0;
}
int calc_Ainvb(double *x_data, double *A_data, double *b_data, int nx, int ndata,int nxmarume)
{
  /*
    b= A x  --> x= Ainv b
     b  in R^{ndata x 1}
     A  in R^{ndata x nx}   (ndata>nx)
     x  in R^{nx    x 1}
   */
  int i;

  if(ndata<=0){
    x_data[0]=1;
    for(i=1;i<nx;i++) x_data[i]=1./nx;
  }
  else{
    gsl_matrix *A =gsl_matrix_alloc(ndata,nx);
    gsl_vector *b =gsl_vector_alloc(ndata);
    //A->data=A_data;
    //	b->data=b_data;
    {
      int n=ndata*nx;
	for(i=0;i<n;i++){
	  A->data[i]=A_data[i];
	}
	for(i=0;i<ndata;i++) b->data[i]=b_data[i];
    }

    gsl_matrix *V = gsl_matrix_alloc (nx,nx);
    gsl_vector *S = gsl_vector_alloc (nx);
    gsl_vector *work = gsl_vector_alloc(nx);
    gsl_vector *x = gsl_vector_alloc(nx);
    gsl_set_error_handler( &least_gsl_error_handler );GSLtag=0;

#if GSL == 1
    gsl_linalg_SV_decomp_jacobi (A, V, S); //higer accuracy than Golub-Reinsh
#elif GSL == 2
    gsl_linalg_SV_decomp (A, V, S, work); //Golub-Reinsh
#elif GSL == 3
    {//faster
      gsl_matrix *X;
      X = gsl_matrix_alloc (nx,nx);
      gsl_linalg_SV_decomp_mod (A, X, V, S, work);
      gsl_matrix_free(X);
    }
#endif
    VarY=0;
    if(GSLtag==1){
      fprintf(stderr,"\nError:EigenValue=");for(i=0;i<nx;i++) fprintf(stderr,"%e ",S->data[i]); fprintf(stderr,"\n");
      for(i=0;i<nx;i++) x_data[i]=1./nx;
    }
    else{
      if(nxmarume>=0){//Principal Component Analysis
	for(i=0;i<nxmarume;i++) VarY+=S->data[i];
	for(i=nxmarume;i<nx;i++) S->data[i]=0;
	gsl_linalg_SV_solve(A,V,S,b,x);
	for(i=0;i<nx;i++) x_data[i]=gsl_vector_get(x,i);
      }
      else{//Minor Component Analysis
	int ii=0;
	int im=-nxmarume;
	for(i=nx-1;i>=0;i--){
	  if(S->data[i]>1e-20){
	    ii++;
	    VarY+=S->data[i];
	    if(ii>=im) break;
	  }
	}
	int j;

	double M0=-V->data[i];
	for(j=1;j<nx;j++){
	  x_data[j-1]=V->data[j*nx+i]/M0;
	}
      }
    }
    gsl_vector_free(S);
    gsl_vector_free(work);
    gsl_vector_free(x);
    gsl_matrix_free(V);
    gsl_matrix_free(A);
    gsl_vector_free(b);
  }
  //  fprintf(stderr,"OK");
  return 0;
}
void calc_AtinvbMCA(double *M, double *At_data, double *b_data, int nx, int ndata,int nxmarume)
{
  /* b= A M  --> M= Ainv b
     b  in R^{ndata x 1}
     A=At  in R^{ndata x nx}
     M  in R^{nx    x 1}
   */
  int nx1=nx+1,i,j,jnx1;
  gsl_matrix *A  =gsl_matrix_alloc(ndata,nx1);
  for(j=0;j<ndata;j++){
    jnx1=j*nx1;
    A->data[jnx1]=b_data[j];
    for(i=0;i<nx;i++){
      A->data[jnx1+i+1]=At_data[i*ndata+j];
    }
  }
  calc_Ainvb(M, (double *)(A->data), b_data, nx1, ndata, -nxmarume);

  gsl_matrix_free(A);
}
void calc_Atinvb(double *M, double *At_data, double *b_data, int nx, int ndata,int nxmarume)
{
  /* b= A M  --> M= Ainv b
     b  in R^{ndata x 1}
     A=At  in R^{ndata x nx}
     M  in R^{nx    x 1}
   */
  gsl_matrix *At =gsl_matrix_alloc(nx,ndata);
  gsl_matrix *A  =gsl_matrix_alloc(ndata,nx);
  At->data=At_data;
  gsl_matrix_transpose_memcpy(A, At);
  calc_Ainvb(M, (double *)(A->data), b_data, nx, ndata, nxmarume);
  gsl_matrix_free(A);
  gsl_matrix_free(At);
}

#endif // __calcAinvb__


/* specfunc/legendre_H3d.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 <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_legendre.h>

#include "error.h"

#include "legendre.h"

/* See [Abbott+Schaefer, Ap.J. 308, 546 (1986)] for
 * enough details to follow what is happening here.
 */

/* Logarithm of normalization factor, Log[N(ell,lambda)].
 * N(ell,lambda) = Product[ lambda^2 + n^2, {n,0,ell} ]
 *               = |Gamma(ell + 1 + I lambda)|^2  lambda sinh(Pi lambda) / Pi
 * Assumes ell >= 0.
 */
static
int
legendre_H3d_lnnorm(const int ell, const double lambda, double * result)
{
  double abs_lam = fabs(lambda);

  if(abs_lam == 0.0) {
    *result = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else if(lambda > (ell + 1.0)/GSL_ROOT3_DBL_EPSILON) {
    /* There is a cancellation between the sinh(Pi lambda)
     * term and the log(gamma(ell + 1 + i lambda) in the
     * result below, so we show some care and save some digits.
     * Note that the above guarantees that lambda is large,
     * since ell >= 0. We use Stirling and a simple expansion
     * of sinh.
     */
    double rat = (ell+1.0)/lambda;
    double ln_lam2ell2  = 2.0*log(lambda) + log(1.0 + rat*rat);
    double lg_corrected = -2.0*(ell+1.0) + M_LNPI + (ell+0.5)*ln_lam2ell2 + 1.0/(288.0*lambda*lambda);
    double angle_terms  = lambda * 2.0 * rat * (1.0 - rat*rat/3.0);
    *result = log(abs_lam) + lg_corrected + angle_terms - M_LNPI;
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result lg_r;
    gsl_sf_result lg_theta;
    gsl_sf_result ln_sinh;
    gsl_sf_lngamma_complex_e(ell+1.0, lambda, &lg_r, &lg_theta);
    gsl_sf_lnsinh_e(M_PI * abs_lam, &ln_sinh);
    *result = log(abs_lam) + ln_sinh.val + 2.0*lg_r.val - M_LNPI;
    return GSL_SUCCESS;
  }
}

/* Calculate series for small eta*lambda.
 * Assumes eta > 0, lambda != 0.
 *
 * This is just the defining hypergeometric for the Legendre function.
 *
 * P^{mu}_{-1/2 + I lam}(z) = 1/Gamma(l+3/2) ((z+1)/(z-1)^(mu/2)
 *                            2F1(1/2 - I lam, 1/2 + I lam; l+3/2; (1-z)/2)
 * We use
 *       z = cosh(eta)
 * (z-1)/2 = sinh^2(eta/2)
 *
 * And recall
 * H3d = sqrt(Pi Norm /(2 lam^2 sinh(eta))) P^{-l-1/2}_{-1/2 + I lam}(cosh(eta))
 */
static
int
legendre_H3d_series(const int ell, const double lambda, const double eta,
                    gsl_sf_result * result)
{
  const int nmax = 5000;
  const double shheta = sinh(0.5*eta);
  const double ln_zp1 = M_LN2 + log(1.0 + shheta*shheta);
  const double ln_zm1 = M_LN2 + 2.0*log(shheta);
  const double zeta = -shheta*shheta;
  gsl_sf_result lg_lp32;
  double term = 1.0;
  double sum  = 1.0;
  double sum_err = 0.0;
  gsl_sf_result lnsheta;
  double lnN;
  double lnpre_val, lnpre_err, lnprepow;
  int stat_e;
  int n;

  gsl_sf_lngamma_e(ell + 3.0/2.0, &lg_lp32);
  gsl_sf_lnsinh_e(eta, &lnsheta);
  legendre_H3d_lnnorm(ell, lambda, &lnN);
  lnprepow = 0.5*(ell + 0.5) * (ln_zm1 - ln_zp1);
  lnpre_val  = lnprepow + 0.5*(lnN + M_LNPI - M_LN2 - lnsheta.val) - lg_lp32.val - log(fabs(lambda));
  lnpre_err  = lnsheta.err + lg_lp32.err + GSL_DBL_EPSILON * fabs(lnpre_val);
  lnpre_err += 2.0*GSL_DBL_EPSILON * (fabs(lnN) + M_LNPI + M_LN2);
  lnpre_err += 2.0*GSL_DBL_EPSILON * (0.5*(ell + 0.5) * (fabs(ln_zm1) + fabs(ln_zp1)));
  for(n=1; n<nmax; n++) {
    double aR = n - 0.5;
    term *= (aR*aR + lambda*lambda)*zeta/(ell + n + 0.5)/n;
    sum  += term;
    sum_err += 2.0*GSL_DBL_EPSILON*fabs(term);
    if(fabs(term/sum) < 2.0 * GSL_DBL_EPSILON) break;
  }

  stat_e = gsl_sf_exp_mult_err_e(lnpre_val, lnpre_err, sum, fabs(term)+sum_err, result);
  return GSL_ERROR_SELECT_2(stat_e, (n==nmax ? GSL_EMAXITER : GSL_SUCCESS));
}

/* Evaluate legendre_H3d(ell+1)/legendre_H3d(ell)
 * by continued fraction.
 */
#if 0
static
int
legendre_H3d_CF1(const int ell, const double lambda, const double coth_eta,
                 gsl_sf_result * result)
{
  const double RECUR_BIG = GSL_SQRT_DBL_MAX;
  const int maxiter = 5000;
  int n = 1;
  double Anm2 = 1.0;
  double Bnm2 = 0.0;
  double Anm1 = 0.0;
  double Bnm1 = 1.0;
  double a1 = hypot(lambda, ell+1.0);
  double b1 = (2.0*ell + 3.0) * coth_eta;
  double An = b1*Anm1 + a1*Anm2;
  double Bn = b1*Bnm1 + a1*Bnm2;
  double an, bn;
  double fn = An/Bn;

  while(n < maxiter) {
    double old_fn;
    double del;
    n++;
    Anm2 = Anm1;
    Bnm2 = Bnm1;
    Anm1 = An;
    Bnm1 = Bn;
    an = -(lambda*lambda + ((double)ell + n)*((double)ell + n));
    bn = (2.0*ell + 2.0*n + 1.0) * coth_eta;
    An = bn*Anm1 + an*Anm2;
    Bn = bn*Bnm1 + an*Bnm2;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An /= RECUR_BIG;
      Bn /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
    }

    old_fn = fn;
    fn = An/Bn;
    del = old_fn/fn;

    if(fabs(del - 1.0) < 4.0*GSL_DBL_EPSILON) break;
  }

  result->val = fn;
  result->err = 2.0 * GSL_DBL_EPSILON * (sqrt(n)+1.0) * fabs(fn);

  if(n >= maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}
#endif /* 0 */

/* Evaluate legendre_H3d(ell+1)/legendre_H3d(ell)
 * by continued fraction. Use the Gautschi (Euler)
 * equivalent series.
 */
 /* FIXME: Maybe we have to worry about this. The a_k are
  * not positive and there can be a blow-up. It happened
  * for J_nu once or twice. Then we should probably use
  * the method above.
  */
static
int
legendre_H3d_CF1_ser(const int ell, const double lambda, const double coth_eta,
                     gsl_sf_result * result)
{
  const double pre = hypot(lambda, ell+1.0)/((2.0*ell+3)*coth_eta);
  const int maxk = 20000;
  double tk   = 1.0;
  double sum  = 1.0;
  double rhok = 0.0;
  double sum_err = 0.0;
  int k;

  for(k=1; k<maxk; k++) {
    double tlk = (2.0*ell + 1.0 + 2.0*k);
    double l1k = (ell + 1.0 + k);
    double ak = -(lambda*lambda + l1k*l1k)/(tlk*(tlk+2.0)*coth_eta*coth_eta);
    rhok = -ak*(1.0 + rhok)/(1.0 + ak*(1.0 + rhok));
    tk  *= rhok;
    sum += tk;
    sum_err += 2.0 * GSL_DBL_EPSILON * k * fabs(tk);
    if(fabs(tk/sum) < GSL_DBL_EPSILON) break;
  }

  result->val  = pre * sum;
  result->err  = fabs(pre * tk);
  result->err += fabs(pre * sum_err);
  result->err += 4.0 * GSL_DBL_EPSILON * fabs(result->val);

  if(k >= maxk)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_legendre_H3d_0_e(const double lambda, const double eta, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(eta < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(eta == 0.0 || lambda == 0.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    const double lam_eta = lambda * eta;
    gsl_sf_result s;
    gsl_sf_sin_err_e(lam_eta, 2.0*GSL_DBL_EPSILON * fabs(lam_eta), &s);
    if(eta > -0.5*GSL_LOG_DBL_EPSILON) {
      double f = 2.0 / lambda * exp(-eta);
      result->val  = f * s.val;
      result->err  = fabs(f * s.val) * (fabs(eta) + 1.0) * GSL_DBL_EPSILON;
      result->err += fabs(f) * s.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }
    else {
      double f = 1.0/(lambda*sinh(eta));
      result->val  = f * s.val;
      result->err  = fabs(f * s.val) * (fabs(eta) + 1.0) * GSL_DBL_EPSILON;
      result->err += fabs(f) * s.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }
    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_H3d_1_e(const double lambda, const double eta, gsl_sf_result * result)
{
  const double xi    = fabs(eta*lambda);
  const double lsq   = lambda*lambda;
  const double lsqp1 = lsq + 1.0;

  /* CHECK_POINTER(result) */

  if(eta < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(eta == 0.0 || lambda == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(xi < GSL_ROOT5_DBL_EPSILON && eta < GSL_ROOT5_DBL_EPSILON) {
    double etasq = eta*eta;
    double xisq  = xi*xi;
    double term1 = (etasq + xisq)/3.0;
    double term2 = -(2.0*etasq*etasq + 5.0*etasq*xisq + 3.0*xisq*xisq)/90.0;
    double sinh_term = 1.0 - eta*eta/6.0 * (1.0 - 7.0/60.0*eta*eta);
    double pre = sinh_term/sqrt(lsqp1) / eta;
    result->val  = pre * (term1 + term2);
    result->err  = pre * GSL_DBL_EPSILON * (fabs(term1) + fabs(term2));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double sin_term;     /*  Sin(xi)/xi     */
    double cos_term;     /*  Cos(xi)        */
    double coth_term;    /*  eta/Tanh(eta)  */
    double sinh_term;    /*  eta/Sinh(eta)  */
    double sin_term_err;
    double cos_term_err;
    double t1;
    double pre_val;
    double pre_err;
    double term1;
    double term2;
    if(xi < GSL_ROOT5_DBL_EPSILON) {
      sin_term = 1.0 - xi*xi/6.0 * (1.0 - xi*xi/20.0);
      cos_term = 1.0 - 0.5*xi*xi * (1.0 - xi*xi/12.0);
      sin_term_err = GSL_DBL_EPSILON;
      cos_term_err = GSL_DBL_EPSILON;
    }
    else {
      gsl_sf_result sin_xi_result;
      gsl_sf_result cos_xi_result;
      gsl_sf_sin_e(xi, &sin_xi_result);
      gsl_sf_cos_e(xi, &cos_xi_result);
      sin_term = sin_xi_result.val/xi;
      cos_term = cos_xi_result.val;
      sin_term_err = sin_xi_result.err/fabs(xi);
      cos_term_err = cos_xi_result.err;
    }
    if(eta < GSL_ROOT5_DBL_EPSILON) {
      coth_term = 1.0 + eta*eta/3.0 * (1.0 - eta*eta/15.0);
      sinh_term = 1.0 - eta*eta/6.0 * (1.0 - 7.0/60.0*eta*eta);
    }
    else {
      coth_term = eta/tanh(eta);
      sinh_term = eta/sinh(eta);
    }
    t1 = sqrt(lsqp1) * eta;
    pre_val = sinh_term/t1;
    pre_err = 2.0 * GSL_DBL_EPSILON * fabs(pre_val);
    term1 = sin_term*coth_term;
    term2 = cos_term;
    result->val  = pre_val * (term1 - term2);
    result->err  = pre_err * fabs(term1 - term2);
    result->err += pre_val * (sin_term_err * coth_term + cos_term_err);
    result->err += pre_val * fabs(term1-term2) * (fabs(eta) + 1.0) * GSL_DBL_EPSILON;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_H3d_e(const int ell, const double lambda, const double eta,
                         gsl_sf_result * result)
{
  const double abs_lam = fabs(lambda);
  const double lsq     = abs_lam*abs_lam;
  const double xi      = abs_lam * eta;
  const double cosh_eta = cosh(eta);

  /* CHECK_POINTER(result) */

  if(eta < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(eta > GSL_LOG_DBL_MAX) {
    /* cosh(eta) is too big. */
    OVERFLOW_ERROR(result);
  }
  else if(ell == 0) {
    return gsl_sf_legendre_H3d_0_e(lambda, eta, result);
  }
  else if(ell == 1) {
    return gsl_sf_legendre_H3d_1_e(lambda, eta, result);
  }
  else if(eta == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(xi < 1.0) {
    return legendre_H3d_series(ell, lambda, eta, result);
  }
  else if((ell*ell+lsq)/sqrt(1.0+lsq)/(cosh_eta*cosh_eta) < 5.0*GSL_ROOT3_DBL_EPSILON) {
    /* Large argument.
     */
    gsl_sf_result P;
    double lm;
    int stat_P = gsl_sf_conicalP_large_x_e(-ell-0.5, lambda, cosh_eta, &P, &lm);
    if(P.val == 0.0) {
      result->val = 0.0;
      result->err = 0.0;
      return stat_P;
    }
    else {
      double lnN;
      gsl_sf_result lnsh;
      double ln_abslam;
      double lnpre_val, lnpre_err;
      int stat_e;
      gsl_sf_lnsinh_e(eta, &lnsh);
      legendre_H3d_lnnorm(ell, lambda, &lnN);
      ln_abslam = log(abs_lam);
      lnpre_val  = 0.5*(M_LNPI + lnN - M_LN2 - lnsh.val) - ln_abslam;
      lnpre_err  = lnsh.err;
      lnpre_err += 2.0 * GSL_DBL_EPSILON * (0.5*(M_LNPI + M_LN2 + fabs(lnN)) + fabs(ln_abslam));
      lnpre_err += 2.0 * GSL_DBL_EPSILON * fabs(lnpre_val);
      stat_e = gsl_sf_exp_mult_err_e(lnpre_val + lm, lnpre_err, P.val, P.err, result);
      return GSL_ERROR_SELECT_2(stat_e, stat_P);
    }
  }
  else if(abs_lam > 1000.0*ell*ell) {
    /* Large degree.
     */
    gsl_sf_result P;
    double lm;
    int stat_P = gsl_sf_conicalP_xgt1_neg_mu_largetau_e(ell+0.5,
                                                           lambda,
                                                           cosh_eta, eta,
                                                           &P, &lm);
    if(P.val == 0.0) {
      result->val = 0.0;
      result->err = 0.0;
      return stat_P;
    }
    else {
      double lnN;
      gsl_sf_result lnsh;
      double ln_abslam;
      double lnpre_val, lnpre_err;
      int stat_e;
      gsl_sf_lnsinh_e(eta, &lnsh);
      legendre_H3d_lnnorm(ell, lambda, &lnN);
      ln_abslam = log(abs_lam);
      lnpre_val  = 0.5*(M_LNPI + lnN - M_LN2 - lnsh.val) - ln_abslam;
      lnpre_err  = lnsh.err;
      lnpre_err += GSL_DBL_EPSILON * (0.5*(M_LNPI + M_LN2 + fabs(lnN)) + fabs(ln_abslam));
      lnpre_err += 2.0 * GSL_DBL_EPSILON * fabs(lnpre_val);
      stat_e = gsl_sf_exp_mult_err_e(lnpre_val + lm, lnpre_err, P.val, P.err, result);
      return GSL_ERROR_SELECT_2(stat_e, stat_P);
    }
  }
  else {
    /* Backward recurrence.
     */
    const double coth_eta = 1.0/tanh(eta);
    const double coth_err_mult = fabs(eta) + 1.0;
    gsl_sf_result rH;
    int stat_CF1 = legendre_H3d_CF1_ser(ell, lambda, coth_eta, &rH);
    double Hlm1;
    double Hl    = GSL_SQRT_DBL_MIN;
    double Hlp1  = rH.val * Hl;
    int lp;
    for(lp=ell; lp>0; lp--) {
      double root_term_0 = hypot(lambda,lp);
      double root_term_1 = hypot(lambda,lp+1.0);
      Hlm1 = ((2.0*lp + 1.0)*coth_eta*Hl - root_term_1 * Hlp1)/root_term_0;
      Hlp1 = Hl;
      Hl   = Hlm1;
    }

    if(fabs(Hl) > fabs(Hlp1)) {
      gsl_sf_result H0;
      int stat_H0 = gsl_sf_legendre_H3d_0_e(lambda, eta, &H0);
      result->val  = GSL_SQRT_DBL_MIN/Hl * H0.val;
      result->err  = GSL_SQRT_DBL_MIN/fabs(Hl) * H0.err;
      result->err += fabs(rH.err/rH.val) * (ell+1.0) * coth_err_mult * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_H0, stat_CF1);
    }
    else {
      gsl_sf_result H1;
      int stat_H1 = gsl_sf_legendre_H3d_1_e(lambda, eta, &H1);
      result->val  = GSL_SQRT_DBL_MIN/Hlp1 * H1.val;
      result->err  = GSL_SQRT_DBL_MIN/fabs(Hlp1) * H1.err;
      result->err += fabs(rH.err/rH.val) * (ell+1.0) * coth_err_mult * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_H1, stat_CF1);
    }
  }
}

int
gsl_sf_legendre_H3d_array(const int lmax, const double lambda, const double eta, double * result_array)
{
  /* CHECK_POINTER(result_array) */

 if(eta < 0.0 || lmax < 0) {
    int ell;
    for(ell=0; ell<=lmax; ell++) result_array[ell] = 0.0;
    GSL_ERROR ("domain error", GSL_EDOM);
  }
  else if(eta > GSL_LOG_DBL_MAX) {
    /* cosh(eta) is too big. */
    int ell;
    for(ell=0; ell<=lmax; ell++) result_array[ell] = 0.0;
    GSL_ERROR ("overflow", GSL_EOVRFLW);
  }
  else if(lmax == 0) {
    gsl_sf_result H0;
    int stat = gsl_sf_legendre_H3d_e(0, lambda, eta, &H0);
    result_array[0] = H0.val;
    return stat;
  }
  else {
    /* Not the most efficient method. But what the hell... it's simple.
     */
    gsl_sf_result r_Hlp1;
    gsl_sf_result r_Hl;
    int stat_lmax   = gsl_sf_legendre_H3d_e(lmax,   lambda, eta, &r_Hlp1);
    int stat_lmaxm1 = gsl_sf_legendre_H3d_e(lmax-1, lambda, eta, &r_Hl);
    int stat_max = GSL_ERROR_SELECT_2(stat_lmax, stat_lmaxm1);

    const double coth_eta = 1.0/tanh(eta);
    int stat_recursion = GSL_SUCCESS;
    double Hlp1 = r_Hlp1.val;
    double Hl   = r_Hl.val;
    double Hlm1;
    int ell;

    result_array[lmax]   = Hlp1;
    result_array[lmax-1] = Hl;

    for(ell=lmax-1; ell>0; ell--) {
      double root_term_0 = hypot(lambda,ell);
      double root_term_1 = hypot(lambda,ell+1.0);
      Hlm1 = ((2.0*ell + 1.0)*coth_eta*Hl - root_term_1 * Hlp1)/root_term_0;
      result_array[ell-1] = Hlm1;
      if(!(Hlm1 < GSL_DBL_MAX)) stat_recursion = GSL_EOVRFLW;
      Hlp1 = Hl;
      Hl   = Hlm1;
    }

    return GSL_ERROR_SELECT_2(stat_recursion, stat_max);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_legendre_H3d_0(const double lambda, const double eta)
{
  EVAL_RESULT(gsl_sf_legendre_H3d_0_e(lambda, eta, &result));
}

double gsl_sf_legendre_H3d_1(const double lambda, const double eta)
{
  EVAL_RESULT(gsl_sf_legendre_H3d_1_e(lambda, eta, &result));
}

double gsl_sf_legendre_H3d(const int l, const double lambda, const double eta)
{
  EVAL_RESULT(gsl_sf_legendre_H3d_e(l, lambda, eta, &result));
}


/* movstat/test_mean.c
 *
 * Copyright (C) 2018 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.
 */

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_movstat.h>

/* compute moving mean by explicitely constructing window and computing mean */
int
slow_movmean(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y,
             const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double mean = gsl_stats_mean(window, 1, wsize);

      gsl_vector_set(y, i, mean);
    }

  free(window);

  return GSL_SUCCESS;
}

static double
func_mean(const size_t n, double x[], void * params)
{
  (void) params;
  return gsl_stats_mean(x, 1, n);
}

static void
test_mean_proc(const double tol, const size_t n, const size_t H, const size_t J,
               const gsl_movstat_end_t etype, gsl_rng * rng_p)
{
  gsl_movstat_workspace * w = gsl_movstat_alloc2(H, J);
  gsl_vector * x = gsl_vector_alloc(n);
  gsl_vector * y = gsl_vector_alloc(n);
  gsl_vector * z = gsl_vector_alloc(n);
  gsl_movstat_function F;
  char buf[2048];

  F.function = func_mean;
  F.params = NULL;

  random_vector(x, rng_p);

  /* y = mean(x) with slow brute force method */
  slow_movmean(etype, x, y, H, J);

  /* y = mean(x) with fast method */
  gsl_movstat_mean(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u mean random", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = mean(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_mean(etype, z, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u mean random in-place", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = mean(x) with user-defined function */
  gsl_movstat_apply(etype, &F, x, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u mean user", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  gsl_movstat_free(w);
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
}

static void
test_mean(gsl_rng * rng_p)
{
  const double eps = 1.0e-10;

  test_mean_proc(eps, 1000, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 1000, 3, 3, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 1000, 0, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 1000, 5, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 2000, 10, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 2000, 5, 10, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 20, 10, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_mean_proc(eps, 20, 50, 10, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_mean_proc(eps, 1000, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 1000, 3, 3, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 1000, 0, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 1000, 5, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 2000, 10, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 2000, 5, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 20, 10, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_mean_proc(eps, 20, 50, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_mean_proc(eps, 1000, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 1000, 3, 3, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 1000, 0, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 1000, 5, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 2000, 10, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 2000, 5, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 20, 10, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_mean_proc(eps, 20, 50, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


/* specfunc/mathieu_radfunc.c
 *
 * Copyright (C) 2002 Lowell Johnson
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  L. Johnson */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_sf_mathieu.h>

int gsl_sf_mathieu_Mc_e(int kind, int order, double qq, double zz,
                      gsl_sf_result *result)
{
  int even_odd, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn, factor;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1pc, z2pc;
  double u1, u2;
  gsl_sf_result aa;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  mm = 0;
  amax = 0.0;
  fn = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

  even_odd = 0;
  if (order % 2 != 0)
      even_odd = 1;

  /* Compute the characteristic value. */
  status = gsl_sf_mathieu_a_e(order, qq, &aa);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  /* Compute the series coefficients. */
  status = gsl_sf_mathieu_a_coeff(order, qq, aa.val, coeff);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  if (even_odd == 0)
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1c = gsl_sf_bessel_Jn(kk, u1);
          if (kind == 1)
          {
              z2c = gsl_sf_bessel_Jn(kk, u2);
          }
          else /* kind = 2 */
          {
              z2c = gsl_sf_bessel_Yn(kk, u2);
          }

          fc = pow(-1.0, 0.5*order+kk)*coeff[kk];
          fn += fc*j1c*z2c;
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }
  else
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1c = gsl_sf_bessel_Jn(kk, u1);
          j1pc = gsl_sf_bessel_Jn(kk+1, u1);
          if (kind == 1)
          {
              z2c = gsl_sf_bessel_Jn(kk, u2);
              z2pc = gsl_sf_bessel_Jn(kk+1, u2);
          }
          else /* kind = 2 */
          {
              z2c = gsl_sf_bessel_Yn(kk, u2);
              z2pc = gsl_sf_bessel_Yn(kk+1, u2);
          }
          fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
          fn += fc*(j1c*z2pc + j1pc*z2c);
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }

  result->val = fn;
  result->err = 2.0*GSL_DBL_EPSILON;
  factor = fabs(fn);
  if (factor > 1.0)
      result->err *= factor;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_Ms_e(int kind, int order, double qq, double zz,
                      gsl_sf_result *result)
{
  int even_odd, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn, factor;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1mc, z2mc, j1pc, z2pc;
  double u1, u2;
  gsl_sf_result aa;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  /* Handle the trivial cases where order = 0. */
  if (order == 0)
  {
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
  }

  mm = 0;
  amax = 0.0;
  fn = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

  even_odd = 0;
  if (order % 2 != 0)
      even_odd = 1;

  /* Compute the characteristic value. */
  status = gsl_sf_mathieu_b_e(order, qq, &aa);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  /* Compute the series coefficients. */
  status = gsl_sf_mathieu_b_coeff(order, qq, aa.val, coeff);
  if (status != GSL_SUCCESS)
  {
      return status;
  }

  if (even_odd == 0)
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1mc = gsl_sf_bessel_Jn(kk, u1);
          j1pc = gsl_sf_bessel_Jn(kk+2, u1);
          if (kind == 1)
          {
              z2mc = gsl_sf_bessel_Jn(kk, u2);
              z2pc = gsl_sf_bessel_Jn(kk+2, u2);
          }
          else /* kind = 2 */
          {
              z2mc = gsl_sf_bessel_Yn(kk, u2);
              z2pc = gsl_sf_bessel_Yn(kk+2, u2);
          }

          fc = pow(-1.0, 0.5*order+kk+1)*coeff[kk];
          fn += fc*(j1mc*z2pc - j1pc*z2mc);
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }
  else
  {
      for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
      {
          amax = GSL_MAX(amax, fabs(coeff[kk]));
          if (fabs(coeff[kk])/amax < maxerr)
              break;

          j1c = gsl_sf_bessel_Jn(kk, u1);
          j1pc = gsl_sf_bessel_Jn(kk+1, u1);
          if (kind == 1)
          {
              z2c = gsl_sf_bessel_Jn(kk, u2);
              z2pc = gsl_sf_bessel_Jn(kk+1, u2);
          }
          else /* kind = 2 */
          {
              z2c = gsl_sf_bessel_Yn(kk, u2);
              z2pc = gsl_sf_bessel_Yn(kk+1, u2);
          }

          fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
          fn += fc*(j1c*z2pc - j1pc*z2c);
      }

      fn *= sqrt(pi/2.0)/coeff[0];
  }

  result->val = fn;
  result->err = 2.0*GSL_DBL_EPSILON;
  factor = fabs(fn);
  if (factor > 1.0)
      result->err *= factor;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_Mc_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[])
{
  int even_odd, order, ii, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1pc, z2pc;
  double u1, u2;
  double *aa = work->aa;

  /* Initialize the result array to zeroes. */
  for (ii=0; ii<nmax-nmin+1; ii++)
      result_array[ii] = 0.0;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  mm = 0;
  amax = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

  /* Compute all eigenvalues up to nmax. */
  gsl_sf_mathieu_a_array(0, nmax, qq, work, aa);

  for (ii=0, order=nmin; order<=nmax; ii++, order++)
  {
      fn = 0.0;
      even_odd = 0;
      if (order % 2 != 0)
          even_odd = 1;

      /* Compute the series coefficients. */
      status = gsl_sf_mathieu_a_coeff(order, qq, aa[order], coeff);
      if (status != GSL_SUCCESS)
      {
          return status;
      }

      if (even_odd == 0)
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1c = gsl_sf_bessel_Jn(kk, u1);
              if (kind == 1)
              {
                  z2c = gsl_sf_bessel_Jn(kk, u2);
              }
              else /* kind = 2 */
              {
                  z2c = gsl_sf_bessel_Yn(kk, u2);
              }

              fc = pow(-1.0, 0.5*order+kk)*coeff[kk];
              fn += fc*j1c*z2c;
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }
      else
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1c = gsl_sf_bessel_Jn(kk, u1);
              j1pc = gsl_sf_bessel_Jn(kk+1, u1);
              if (kind == 1)
              {
                  z2c = gsl_sf_bessel_Jn(kk, u2);
                  z2pc = gsl_sf_bessel_Jn(kk+1, u2);
              }
              else /* kind = 2 */
              {
                  z2c = gsl_sf_bessel_Yn(kk, u2);
                  z2pc = gsl_sf_bessel_Yn(kk+1, u2);
              }
              fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
              fn += fc*(j1c*z2pc + j1pc*z2c);
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }

      result_array[ii] = fn;
  } /* order loop */

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_Ms_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[])
{
  int even_odd, order, ii, kk, mm, status;
  double maxerr = 1e-14, amax, pi = M_PI, fn;
  double coeff[GSL_SF_MATHIEU_COEFF], fc;
  double j1c, z2c, j1mc, z2mc, j1pc, z2pc;
  double u1, u2;
  double *bb = work->bb;

  /* Initialize the result array to zeroes. */
  for (ii=0; ii<nmax-nmin+1; ii++)
      result_array[ii] = 0.0;

  /* Check for out of bounds parameters. */
  if (qq <= 0.0)
  {
      GSL_ERROR("q must be greater than zero", GSL_EINVAL);
  }
  if (kind < 1 || kind > 2)
  {
      GSL_ERROR("kind must be 1 or 2", GSL_EINVAL);
  }

  mm = 0;
  amax = 0.0;
  u1 = sqrt(qq)*exp(-1.0*zz);
  u2 = sqrt(qq)*exp(zz);

  /* Compute all eigenvalues up to nmax. */
  gsl_sf_mathieu_b_array(0, nmax, qq, work, bb);

  for (ii=0, order=nmin; order<=nmax; ii++, order++)
  {
      fn = 0.0;
      even_odd = 0;
      if (order % 2 != 0)
          even_odd = 1;

      /* Handle the trivial cases where order = 0. */
      if (order == 0)
      {
          result_array[ii] = 0.0;
          continue;
      }

      /* Compute the series coefficients. */
      status = gsl_sf_mathieu_b_coeff(order, qq, bb[order], coeff);
      if (status != GSL_SUCCESS)
      {
          return status;
      }

      if (even_odd == 0)
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1mc = gsl_sf_bessel_Jn(kk, u1);
              j1pc = gsl_sf_bessel_Jn(kk+2, u1);
              if (kind == 1)
              {
                  z2mc = gsl_sf_bessel_Jn(kk, u2);
                  z2pc = gsl_sf_bessel_Jn(kk+2, u2);
              }
              else /* kind = 2 */
              {
                  z2mc = gsl_sf_bessel_Yn(kk, u2);
                  z2pc = gsl_sf_bessel_Yn(kk+2, u2);
              }

              fc = pow(-1.0, 0.5*order+kk+1)*coeff[kk];
              fn += fc*(j1mc*z2pc - j1pc*z2mc);
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }
      else
      {
          for (kk=0; kk<GSL_SF_MATHIEU_COEFF; kk++)
          {
              amax = GSL_MAX(amax, fabs(coeff[kk]));
              if (fabs(coeff[kk])/amax < maxerr)
                  break;

              j1c = gsl_sf_bessel_Jn(kk, u1);
              j1pc = gsl_sf_bessel_Jn(kk+1, u1);
              if (kind == 1)
              {
                  z2c = gsl_sf_bessel_Jn(kk, u2);
                  z2pc = gsl_sf_bessel_Jn(kk+1, u2);
              }
              else /* kind = 2 */
              {
                  z2c = gsl_sf_bessel_Yn(kk, u2);
                  z2pc = gsl_sf_bessel_Yn(kk+1, u2);
              }

              fc = pow(-1.0, 0.5*(order-1)+kk)*coeff[kk];
              fn += fc*(j1c*z2pc - j1pc*z2c);
          }

          fn *= sqrt(pi/2.0)/coeff[0];
      }

      result_array[ii] = fn;
  } /* order loop */

  return GSL_SUCCESS;
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_mathieu_Mc(int kind, int order, double qq, double zz)
{
	EVAL_RESULT(gsl_sf_mathieu_Mc_e(kind, order, qq, zz, &result));
}

double gsl_sf_mathieu_Ms(int kind, int order, double qq, double zz)
{
	EVAL_RESULT(gsl_sf_mathieu_Ms_e(kind, order, qq, zz, &result));
}


/* Cholesky Decomposition
 *
 * Copyright (C) 2000  Thomas Walter
 *
 * 3 May 2000: Modified for GSL by Brian Gough
 *
 * This is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2, or (at your option) any
 * later version.
 *
 * This source 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.
 */

/*
 * Cholesky decomposition of a symmetrix positive definite matrix.
 * This is useful to solve the matrix arising in
 *    periodic cubic splines
 *    approximating splines
 *
 * This algorthm does:
 *   A = L * L'
 * with
 *   L  := lower left triangle matrix
 *   L' := the transposed form of L.
 *
 */

#include <config.h>

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

int
gsl_linalg_cholesky_decomp (gsl_matrix * A)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i,j,k;
      int status = 0;

      /* Do the first 2 rows explicitly.  It is simple, and faster.  And
       * one can return if the matrix has only 1 or 2 rows.
       */

      double A_00 = gsl_matrix_get (A, 0, 0);

      double L_00 = sqrt(A_00);

      if (A_00 <= 0)
        {
          status = GSL_EDOM ;
        }

      gsl_matrix_set (A, 0, 0, L_00);

      if (M > 1)
        {
          double A_10 = gsl_matrix_get (A, 1, 0);
          double A_11 = gsl_matrix_get (A, 1, 1);

          double L_10 = A_10 / L_00;
          double diag = A_11 - L_10 * L_10;
          double L_11 = sqrt(diag);

          if (diag <= 0)
            {
              status = GSL_EDOM;
            }

          gsl_matrix_set (A, 1, 0, L_10);
          gsl_matrix_set (A, 1, 1, L_11);
        }

      for (k = 2; k < M; k++)
        {
          double A_kk = gsl_matrix_get (A, k, k);

          for (i = 0; i < k; i++)
            {
              double sum = 0;

              double A_ki = gsl_matrix_get (A, k, i);
              double A_ii = gsl_matrix_get (A, i, i);

              gsl_vector_view ci = gsl_matrix_row (A, i);
              gsl_vector_view ck = gsl_matrix_row (A, k);

              if (i > 0) {
                gsl_vector_view di = gsl_vector_subvector(&ci.vector, 0, i);
                gsl_vector_view dk = gsl_vector_subvector(&ck.vector, 0, i);

                gsl_blas_ddot (&di.vector, &dk.vector, &sum);
              }

              A_ki = (A_ki - sum) / A_ii;
              gsl_matrix_set (A, k, i, A_ki);
            }

          {
            gsl_vector_view ck = gsl_matrix_row (A, k);
            gsl_vector_view dk = gsl_vector_subvector (&ck.vector, 0, k);

            double sum = gsl_blas_dnrm2 (&dk.vector);
            double diag = A_kk - sum * sum;

            double L_kk = sqrt(diag);

            if (diag <= 0)
              {
                status = GSL_EDOM;
              }

            gsl_matrix_set (A, k, k, L_kk);
          }
        }

      /* Now copy the transposed lower triangle to the upper triangle,
       * the diagonal is common.
       */

      for (i = 1; i < M; i++)
        {
          for (j = 0; j < i; j++)
            {
              double A_ij = gsl_matrix_get (A, i, j);
              gsl_matrix_set (A, j, i, A_ij);
            }
        }

      if (status == GSL_EDOM)
        {
          GSL_ERROR ("matrix must be positive definite", GSL_EDOM);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_solve (const gsl_matrix * LLT,
                           const gsl_vector * b,
                           gsl_vector * x)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (LLT->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve for c using forward-substitution, L c = b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasNonUnit, LLT, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LLT, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_svx (const gsl_matrix * LLT,
                         gsl_vector * x)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (LLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Solve for c using forward-substitution, L c = b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasNonUnit, LLT, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LLT, x);

      return GSL_SUCCESS;
    }
}