/****************************************************************
  wigner_gsl.h / wigner_gsl_twice.h

  Defines Wigner coupling and recoupling symbols as wrappers for GSL
  angular momentum functions:

    - wigner_gsl.h -- takes HalfInt angular momentum arguments (RECOMMENDED)
    - wigner_gsl_twice.h -- takes integer "twice value" angular momentum arguments

  Naming convention:
    - Function names *not* ending in '2' accept HalfInt arguments J.
    - Function names ending in '2' accept integer arguments 2*J.

  See, e.g., appendix to de Shalit and Talmi for underlying formulas.

  DOCUMENTATION: See wigner_gsl.h (rather than wigner_gsl_twice.h) for
  more complete function comments.

  Language: C++

  Mark A. Caprio
  University of Notre Dame

  2/16/10 (mac): Created.
  11/13/15 (mac): Add unitary 6J for (12)3-(13)2 recoupling
    and Racah reduction factor.
  2/27/16 (mac): Update includes for restructured header files.
  3/8/16 (mac): Enclose in namespace.
  6/8/16 (mac): Update #define guard directive.
  6/21/16 (mac): Remove Racah reduction factor. Update comments.
  10/18/16 (mac): Update Unitary6J comment. Rename wigner2_gsl.h to
    wigner_gsl_twice.h.
  4/28/18 (mac): Restore missing Hat2 and ParitySign2 to
    wigner_gsl_twice.h.

****************************************************************/

#ifndef WIGNER_GSL_H_
#define WIGNER_GSL_H_

#include <gsl/gsl_sf_coupling.h>

#include "am.h"

namespace am {

  // Wigner3J(ja,jb,jc,ma,mb,mc)
  //   returns Wigner 3-J symbol
  //   wrapper for gsl_sf_coupling_3j

  inline
    double Wigner3J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& ma, const HalfInt& mb, const HalfInt& mc
      )
  {
    return gsl_sf_coupling_3j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(ma), TwiceValue(mb), TwiceValue(mc)
      );
  }

  // ClebschGordan(ja,ma,jb,mb,jc,mc)
  //   returns Clebsch-Gordan coefficient
  //   wrapper for gsl_sf_coupling_3j

  inline
    double ClebschGordan(
        const HalfInt& ja, const HalfInt& ma,
        const HalfInt& jb, const HalfInt& mb,
        const HalfInt& jc, const HalfInt& mc
      )
  {
    return Hat(jc)*ParitySign(ja-jb+mc)
      *gsl_sf_coupling_3j(
          TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
          TwiceValue(ma), TwiceValue(mb), -TwiceValue(mc)
        );
  }

  // Wigner6J(ja,jb,jc,jd,je,jf)
  //   returns Wigner 6-J symbol
  //   wrapper for gsl_sf_coupling_6j

  inline
    double Wigner6J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return gsl_sf_coupling_6j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(jd), TwiceValue(je), TwiceValue(jf)
      );
  }

  // Unitary6J(ja,jb,jc,jd,je,jf)
  //   wrapper for gsl_sf_coupling_6j
  //   returns unitary recoupling symbol for (12)3-1(23) recoupling
  //
  //   Arguments follow row order in 6J symbol:
  //     Unitary6J(J1,J2,J12,J3,J,J23)
  //   This is equivalent to U(J1,J2,J,J3,J12,J23), though neither is a
  //   particularly memorable ordering.  Why not (J1,J2,J3,J12,J23,J)?!

  inline
    double Unitary6J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return ParitySign(ja+jb+jd+je)*Hat(jc)*Hat(jf)
      *Wigner6J(ja,jb,jc,jd,je,jf);
  }

  // Unitary6JZ(ja,jb,jc,jd,je,jf)
  //   wrapper for gsl_sf_coupling_6j
  //   returns unitary recoupling symbol for (12)3-(13)2 recoupling
  //   the name "Z" follows Millener for this purpose
  //
  //   Arguments follow row order in 6J symbol:
  //     Unitary6JZ(J1,J2,J12,J,J3,J13)
  //   This is equivalent to Z(J2,J1,J,J3,J12,J13), though neither is a
  //   particularly memorable ordering.  Why not (J1,J2,J3,J12,J13,J)?!

  inline
    double Unitary6JZ(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return ParitySign(jb+je+jc+jf)*Hat(jc)*Hat(jf)
      *Wigner6J(ja,jb,jc,jd,je,jf);
  }

  // Wigner9J(ja,jb,jc,jd,je,jf,jg,jh,ji)
  //   returns Wigner 9-J symbol
  //   wrapper for gsl_sf_coupling_9j

  inline
    double Wigner9J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf,
        const HalfInt& jg, const HalfInt& jh, const HalfInt& ji
      )
  {
    return gsl_sf_coupling_9j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(jd), TwiceValue(je), TwiceValue(jf),
        TwiceValue(jg), TwiceValue(jh), TwiceValue(ji)
      );
  }

  // Unitary9J(ja,jb,jc,jd,je,jf,jg,jh,ji)
  //   returns unitary 9-J symbol
  //   wrapper for gsl_sf_coupling_9j

  inline
    double Unitary9J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf,
        const HalfInt& jg, const HalfInt& jh, const HalfInt& ji
      )
  {
    return Hat(jc)*Hat(jf)*Hat(jg)*Hat(jh)
      *Wigner9J(
          ja, jb, jc,
          jd, je, jf,
          jg, jh, ji
        );
  }

} // namespace

#endif


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>

int
main(void)
{
  size_t i;
  const size_t N = 9;

  /* this dataset is taken from
   * J. M. Hyman, Accurate Monotonicity preserving cubic interpolation,
   * SIAM J. Sci. Stat. Comput. 4, 4, 1983. */
  const double x[] = { 7.99, 8.09, 8.19, 8.7, 9.2,
                       10.0, 12.0, 15.0, 20.0 };
  const double y[] = { 0.0, 2.76429e-5, 4.37498e-2,
                       0.169183, 0.469428, 0.943740,
                       0.998636, 0.999919, 0.999994 };

  gsl_interp_accel *acc = gsl_interp_accel_alloc();
  gsl_spline *spline_cubic = gsl_spline_alloc(gsl_interp_cspline, N);
  gsl_spline *spline_akima = gsl_spline_alloc(gsl_interp_akima, N);
  gsl_spline *spline_steffen = gsl_spline_alloc(gsl_interp_steffen, N);

  gsl_spline_init(spline_cubic, x, y, N);
  gsl_spline_init(spline_akima, x, y, N);
  gsl_spline_init(spline_steffen, x, y, N);

  for (i = 0; i < N; ++i)
    printf("%g %g\n", x[i], y[i]);

  printf("\n\n");

  for (i = 0; i <= 100; ++i)
    {
      double xi = (1 - i / 100.0) * x[0] + (i / 100.0) * x[N-1];
      double yi_cubic = gsl_spline_eval(spline_cubic, xi, acc);
      double yi_akima = gsl_spline_eval(spline_akima, xi, acc);
      double yi_steffen = gsl_spline_eval(spline_steffen, xi, acc);

      printf("%g %g %g %g\n", xi, yi_cubic, yi_akima, yi_steffen);
    }

  gsl_spline_free(spline_cubic);
  gsl_spline_free(spline_akima);
  gsl_spline_free(spline_steffen);
  gsl_interp_accel_free(acc);

  return 0;
}


//
//  her.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 12/23/11.
//  Copyright (c) 2011 University of Colorado Denver. All rights reserved.
//

#ifndef _her_h
#define _her_h

/// @file her.h Performs complex vector outer product.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Performs a vector outer product of a complex vector with itself.
   ///
   /// For a complex Hermitian matrix A, complex vector x and real scalar alpha,
   ///
   ///        A := alpha * x * x' + A
   ///
   /// is computed.  The result is Hermitian and is stored as either upper or lower triangular in A.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   ///
   /// @param n Specifies the order of the matrix A.  n>=0
   /// @param alpha Real scalar.
   /// @param x Pointer to complex vector x.
   /// @param incx Increment of the vector x.  x!=0
   /// @param A Pointer to complex Hermitian n-by-n matrix A.  If uplo = 'U' or 'u' then only the upper
   /// triangular part of A is referenced and the lower part is not referenced.  If uplo = 'L' or 'l' then
   /// only the lower triangular part of A is referenced and the upper part is not referenced.
   /// @param ldA Column length of matrix A.  ldA>=n.
   /// @ingroup BLAS

   template <typename real_t>
   int HER(char uplo, int_t n, real_t alpha, complex<real_t> *x, int_t incx, complex<real_t> *A, int_t ldA)
   {
      using std::conj;
      using std::real;
      using std::toupper;
      const real_t zero(0.0);
      complex<real_t> t;
      int_t i,j,kx,jx,ix;

      uplo=toupper(uplo);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;
      else if((n==0)||(alpha==zero))
         return 0;

      if(incx==1)
      {
         if(uplo=='U')
         {
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[j]);
               for(i=0;i<j;i++)
                  A[i]+=x[i]*t;
               A[j]=real(A[j])+real(x[j]*t);
               A+=ldA;
            }
         }
         else
         {
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[j]);
               A[j]=real(A[j])+real(t*x[j]);
               for(i=j+1;i<n;i++)
                  A[i]+=x[i]*t;
               A+=ldA;
            }
         }
      }
      else
      {
         kx=(incx>0)?0:(1-n)*incx;
         if(uplo=='U')
         {
            jx=kx;
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[jx]);
               ix=kx;
               for(i=0;i<j;i++)
               {
                  A[i]+=x[ix]*t;
                  ix+=incx;
               }
               A[j]=real(A[j])+real(x[jx]*t);
               A+=ldA;
               jx+=incx;
            }
         }
         else
         {
            jx=kx;
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[jx]);
               A[j]=real(A[j])+real(t*x[jx]);
               ix=jx;
               for(i=j+1;i<n;i++)
               {
                  ix+=incx;
                  A[i]+=x[ix]*t;
               }
               jx+=incx;
               A+=ldA;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int HER<float>(char uplo, int_t n, float alpha, complex<float> *x, int_t incx, complex<float> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_cher(CblasColMajor,Uplo,n,alpha,x,incx,A,ldA);

      return 0;
   }

   template <> int HER<double>(char uplo, int_t n, double alpha, complex<double> *x, int_t incx, complex<double> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_zher(CblasColMajor,Uplo,n,alpha,x,incx,A,ldA);

      return 0;
   }
#endif
}
#endif


#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>
#include <string.h>

struct quadratic_params
{
    double a, b, c;
};

double
quadratic (double x, void *params)
{
    struct quadratic_params *p
            = (struct quadratic_params *) params;

    double a = p->a;
    double b = p->b;
    double c = p->c;

    return (a * x + b) * x + c;
}

double
quadratic_deriv (double x, void *params)
{
    struct quadratic_params *p
            = (struct quadratic_params *) params;

    double a = p->a;
    double b = p->b;

    return 2.0 * a * x + b;
}

void
quadratic_fdf (double x, void *params,
               double *y, double *dy)
{
    struct quadratic_params *p
            = (struct quadratic_params *) params;

    double a = p->a;
    double b = p->b;
    double c = p->c;

    *y = (a * x + b) * x + c;
    *dy = 2.0 * a * x + b;
}

int
main (int argc, char *argv[])
{
    int status;
    int iter = 0, max_iter = 100;
    const gsl_root_fdfsolver_type *T;
    gsl_root_fdfsolver *s;
    double x0, x = 5.0, r_expected = sqrt (5.0);
    gsl_function_fdf FDF;
    struct quadratic_params params = {1.0, -2.0, 1.0};

    FDF.f = &quadratic;
    FDF.df = &quadratic_deriv;
    FDF.fdf = &quadratic_fdf;
    FDF.params = &params;

    if (argc < 2) {
        errno = EINVAL;
        perror("");
        return errno;
    }
    char *method = argv[1];
    if (0 == strcmp("newton", method)) {
        T = gsl_root_fdfsolver_newton;
    } else if (0 == strcmp("secant", method)) {
        T = gsl_root_fdfsolver_secant;
    } else if (0 == strcmp("steffenson", method)) {
        T = gsl_root_fdfsolver_steffenson;
    } else {
        errno = EINVAL;
        perror("");
        return errno;
    }
    s = gsl_root_fdfsolver_alloc (T);
    gsl_root_fdfsolver_set (s, &FDF, x);

    printf ("using %s method\n",
            gsl_root_fdfsolver_name (s));

    printf ("%-5s %10s %10s %10s\n",
            "iter", "root", "err", "err(est)");
    do
    {
        iter++;
        status = gsl_root_fdfsolver_iterate (s);
        x0 = x;
        x = gsl_root_fdfsolver_root (s);
        status = gsl_root_test_delta (x, x0, 0, 1e-3);

        if (status == GSL_SUCCESS)
            printf ("Converged:\n");

        printf ("%5d %10.7f %+10.7f %10.7f\n",
                iter, x, x - r_expected, x - x0);
    }
    while (status == GSL_CONTINUE && iter < max_iter);

    gsl_root_fdfsolver_free (s);
    return status;
}


#include <stdio.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_rng.h>

#define NUM 5

int main() {
  int i;
  double data[NUM];
  double mean, variance, largest, smallest;

  gsl_rng* r = gsl_rng_alloc(gsl_rng_default);
  for (i = 0; i < NUM; ++i) data[i] = gsl_rng_uniform(r);

  mean = gsl_stats_mean(data, 1, NUM);
  variance = gsl_stats_variance(data, 1, NUM);
  largest = gsl_stats_max(data, 1, NUM);
  smallest = gsl_stats_min(data, 1, NUM);

  printf("The data set is");
  for (i = 0; i < NUM; ++i) printf(" %6.3f", data[i]);
  printf("\n");

  printf("Mean:     %6.3f\n", mean);
  printf("Variance: %6.3f\n", variance);
  printf("Max value:%6.3f\n", largest);
  printf("Min value:%6.3f\n", smallest);

  return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_chebyshev.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_blas_types.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_randist.h>

double funcExp(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    return exp(pow(x, 2));
}

double funcAbs(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    return fabs(x + pow(x, 3));
}

double funcSign(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    if (x == 0) return 0.0;
    else if (x < 0) return -1.0;

    return 1.0;
}

void calculateExp()
{
    const int stepsCount = 1000;
    FILE* outputR1 = fopen("cheb_exp_r1.txt", "w");
    FILE* outputR2 = fopen("cheb_exp_r2.txt", "w");
    FILE* outputR4 = fopen("cheb_exp_r4.txt", "w");
    FILE* outputR8 = fopen("cheb_exp_r8.txt", "w");
    FILE* outputR16 = fopen("cheb_exp_r16.txt", "w");
    FILE* outputR25 = fopen("cheb_exp_r25.txt", "w");
    FILE* outputR40 = fopen("cheb_exp_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);

    gsl_function function;

    function.function = funcExp;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r1 = GSL_FN_EVAL(&function, x);
        double r2 = gsl_cheb_eval_n(cs, 2, x);
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r8 = gsl_cheb_eval_n(cs, 8, x);
        double r16 = gsl_cheb_eval_n(cs, 16, x);
        double r25 = gsl_cheb_eval_n(cs, 25, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);

        fprintf (outputR1,"%g %g\n", x, r1);
        fprintf (outputR2,"%g %g\n", x, r2);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR8,"%g %g\n", x, r8);
        fprintf (outputR16,"%g %g\n", x, r16);
        fprintf (outputR25,"%g %g\n", x, r25);
        fprintf (outputR40,"%g %g\n", x, r40);
    }

    gsl_cheb_free(cs);
}

void calculateAbs()
{
    const int stepsCount = 1000;
    FILE* outputR1 = fopen("cheb_abs_r1.txt", "w");
    FILE* outputR2 = fopen("cheb_abs_r2.txt", "w");
    FILE* outputR4 = fopen("cheb_abs_r4.txt", "w");
    FILE* outputR8 = fopen("cheb_abs_r8.txt", "w");
    FILE* outputR16 = fopen("cheb_abs_r16.txt", "w");
    FILE* outputR25 = fopen("cheb_abs_r25.txt", "w");
    FILE* outputR40 = fopen("cheb_abs_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);

    gsl_function function;

    function.function = funcAbs;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r1 = GSL_FN_EVAL(&function, x);
        double r2 = gsl_cheb_eval_n(cs, 2, x);
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r8 = gsl_cheb_eval_n(cs, 8, x);
        double r16 = gsl_cheb_eval_n(cs, 16, x);
        double r25 = gsl_cheb_eval_n(cs, 25, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);

        fprintf (outputR1,"%g %g\n", x, r1);
        fprintf (outputR2,"%g %g\n", x, r2);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR8,"%g %g\n", x, r8);
        fprintf (outputR16,"%g %g\n", x, r16);
        fprintf (outputR25,"%g %g\n", x, r25);
        fprintf (outputR40,"%g %g\n", x, r40);
    }

    gsl_cheb_free(cs);
}

void calculateSign()
{
    const int stepsCount = 1000;
    FILE* outputR1 = fopen("cheb_sign_r1.txt", "w");
    FILE* outputR2 = fopen("cheb_sign_r2.txt", "w");
    FILE* outputR4 = fopen("cheb_sign_r4.txt", "w");
    FILE* outputR8 = fopen("cheb_sign_r8.txt", "w");
    FILE* outputR16 = fopen("cheb_sign_r16.txt", "w");
    FILE* outputR25 = fopen("cheb_sign_r25.txt", "w");
    FILE* outputR40 = fopen("cheb_sign_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);

    gsl_function function;

    function.function = funcSign;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r1 = GSL_FN_EVAL(&function, x);
        double r2 = gsl_cheb_eval_n(cs, 2, x);
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r8 = gsl_cheb_eval_n(cs, 8, x);
        double r16 = gsl_cheb_eval_n(cs, 16, x);
        double r25 = gsl_cheb_eval_n(cs, 25, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);

        fprintf (outputR1,"%g %g\n", x, r1);
        fprintf (outputR2,"%g %g\n", x, r2);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR8,"%g %g\n", x, r8);
        fprintf (outputR16,"%g %g\n", x, r16);
        fprintf (outputR25,"%g %g\n", x, r25);
        fprintf (outputR40,"%g %g\n", x, r40);
    }

    gsl_cheb_free(cs);
}

void fitLinear()
{
    const int stepsCount = 1000;
    double xArray[stepsCount * 2];
    double yArray[stepsCount * 2];

    FILE* outputOriginal = fopen("linear_original.txt", "w");
    FILE* outputFit = fopen("linear_fit.txt", "w");
    FILE* outputCheb = fopen("linear_cheb.txt", "w");

    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double y = pow(0.5, pow(x, 2) + 2 * x);

        xArray[i + stepsCount] = x;
        yArray[i + stepsCount] = y;

        fprintf (outputOriginal,"%g %g\n", x, y);
    }

    double c0 = 0;
    double c1 = 0;
    double cov00 = 0;
    double cov01 = 0;
    double cov11 = 0;
    double sumsq = 0;
    gsl_fit_linear(xArray, 1, yArray, 1, stepsCount * 2, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);

//------
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double y = c0 + c1 * x;

        xArray[i + stepsCount] = x;
        yArray[i + stepsCount] = y;

        fprintf (outputFit,"%g %g\n", x, y);
    }

    printf("c0: %f --- c1: %f\n", c0, c1);
}

double funcCheb(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    return pow(0.5, pow(x, 2) + 2 * x);
}

void fitCheb()
{
    const int stepsCount = 1000;
    FILE* outputR4 = fopen("cheb_fit_r4.txt", "w");
    FILE* outputR40 = fopen("cheb_fit_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);
    gsl_function function;
    function.function = funcCheb;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR40,"%g %g\n", x, r40);
    }
    gsl_cheb_free(cs);
}

int main (int argc, char* argv[])
{
    calculateExp();
    calculateSign();
    calculateAbs();
    fitLinear();
    fitCheb();
    return 0;
}

/*

plot "cheb_exp_r1.txt" with lines, \
    "cheb_exp_r2.txt" with lines, \
    "cheb_exp_r4.txt" with lines, \
    "cheb_exp_r8.txt" with lines, \
    "cheb_exp_r16.txt" with lines, \
    "cheb_exp_r25.txt" with lines, \
    "cheb_exp_r40.txt" with lines

plot "cheb_abs_r1.txt" with lines, \
    "cheb_abs_r2.txt" with lines, \
    "cheb_abs_r4.txt" with lines, \
    "cheb_abs_r8.txt" with lines, \
    "cheb_abs_r16.txt" with lines, \
    "cheb_abs_r25.txt" with lines, \
    "cheb_abs_r40.txt" with lines

plot "cheb_sign_r1.txt" with lines, \
    "cheb_sign_r2.txt" with lines, \
    "cheb_sign_r4.txt" with lines, \
    "cheb_sign_r8.txt" with lines, \
    "cheb_sign_r16.txt" with lines, \
    "cheb_sign_r25.txt" with lines, \
    "cheb_sign_r40.txt" with lines

================

plot "linear_original.txt" with lines title 'Original', \
    "linear_fit.txt" with lines title 'Linear least sqr fit', \
    "cheb_fit_r4.txt" with lines title 'Chebyshev approx R4'

*/


#include <stdio.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_matrix_double.h>
// #include "covar.h"

void print_mat(gsl_matrix* C)
{
    int i, j;
    for (i = 0; i < C->size1; i++)
    {
        printf("\n");
        for (j = 0; j < C->size2; j++)
        {
            printf("%f", C->data[C->size2*i+j]);
            if(j < C->size2 - 1) printf(", ");
        }
    }
    printf("\n");
}

int main()
{
    int i, j;

/*
    Okay, here's what we want to do
    1)  Collect *all* the data - 10 seconds at standstill should be sufficient, with
            x, y, z, yaw, pitch, roll; for both pose and twist
    2)  twist gives us euler angles (yaw pitch roll), but pose gives us quaternions,
            so we'll need to convert
    2)  This leads to a 6 by N matrix of values
    3)  We propose a cov_gen() function
            input: 6 by N matrix of observation values
            output: 6 by 6 matrix of covariance values
    4) We can put a ROS wrapper around this function as a service routine,
            i.e. the output of cov_gen() will serve as our estimate of
            the covariance matrix for loam_velodyne's odometry topic
 */

    // const int rsize = 5;
    // const int csize = 3;
    #define rsize 8
    #define csize 6

    double dataA[rsize][csize] =
        {{9, 6, 9, 3, 2, 1},
         {9, 9, 3, 4, 3, 5},
         {6, 6, 6, 6, 7, 3},
         {6, 6, 9, 4, 5, 1},
         {9, 9, 3, 4, 3, 5},
         {6, 6, 6, 6, 7, 3},
         {6, 6, 9, 4, 5, 1},
         {3, 3, 3, 6, 4, 1}};

    // double dataA[rsize][csize] =
    //     {{90, 60, 90},
    //      {90, 90, 30},
    //      {60, 60, 60},
    //      {60, 60, 90},
    //      {30, 30, 30}};

    // double dataA[5][3] =   {{ 24,   0,  30},
    //                         { 24,  30, -30},
    //                         { -6,   0,   0},
    //                         { -6,   0,  30},
    //                         {-36, -30, -30}};

    // double dataA[6][6] = {{1,1,1,1,1,1},
    //                       {2,2,2,2,2,2},
    //                       {3,3,3,3,3,3},
    //                       {4,4,4,4,4,4},
    //                       {5,5,5,5,5,5},
    //                       {6,6,6,6,6,6}};

    gsl_vector_view a, b;

    gsl_matrix *A, *C;
    A = gsl_matrix_alloc(rsize, csize);
    C = gsl_matrix_alloc(csize, csize);

    for ( i = 0; i < rsize; i++)
        for ( j = 0; j < csize; j++)
            gsl_matrix_set (A, i, j, dataA[i][j]);

    print_mat(A);

    for (i = 0; i < A->size2; i++) {
        for (j = 0; j < A->size2; j++) {
          a = gsl_matrix_column (A, i);
          b = gsl_matrix_column (A, j);
          double cov = gsl_stats_covariance(a.vector.data,
                                            a.vector.stride,
                                            b.vector.data,
                                            b.vector.stride,
                                            rsize);
          gsl_matrix_set (C, i, j, cov);
        }
      }

    print_mat(C);

    return 0;
}


/*----------------------------------------------------------------------------*/
/*                                                                            */
/*  quad_golden.c                                                             */
/*                                                                            */
/*  Copyright (C) 2007 James Howse                                            */
/*  Copyright (C) 2009 Brian Gough                                             */
/*                                                                            */
/*  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 algorithm performs univariate minimization (i.e., line search).      */
/*  It requires only objective function values g(x) to compute the minimum.   */
/*  The algorithm maintains an interval of uncertainty [a,b] and a point x    */
/*  in the interval [a,b] such that a < x < b, and g(a) > g(x) and            */
/*  g(x) < g(b).  The algorithm also maintains the three points with the      */
/*  smallest objective values x, v and w such that g(x) < g(v) < g(w).  The   */
/*  algorithm terminates when max( x - a, b - x ) <  2(r |x| + t) where r     */
/*  and t are small positive reals.  At a given iteration, the algorithm      */
/*  first fits a quadratic through the three points (x, g(x)), (v, g(v))      */
/*  and (w, g(w)) and computes the location of the minimum u of the           */
/*  resulting quadratic.  If u is in the interval [a,b] then g(u) is          */
/*  computed.  If u is not in the interval [a,b], and either v < x and        */
/*  w < x, or v > x and w > x (i.e., the quadratic is extrapolating), then    */
/*  a point u' is computed using a safeguarding procedure and g(u') is        */
/*  computed.  If u is not in the interval [a,b], and the quadratic is not    */
/*  extrapolating, then a point u'' is computed using approximate golden      */
/*  section and g(u'') is computed.  After evaluating g() at the              */
/*  appropriate new point, a, b, x, v, and w are updated and the next         */
/*  iteration is performed.  The algorithm is based on work presented in      */
/*  the following references.                                                 */
/*                                                                            */
/*  Algorithms for Minimization without derivatives                           */
/*  Richard Brent                                                             */
/*  Prentice-Hall Inc., Englewood Cliffs, NJ, 1973                            */
/*                                                                            */
/*  Safeguarded Steplength Algorithms for Optimization using Descent Methods  */
/*  Philip E. Gill and Walter Murray                                          */
/*  Division of Numerical Analysis and Computing                              */
/*  National Physical Laboratory, Teddington, United Kingdom                  */
/*  NPL Report NAC 37, August 1974                                            */
/*                                                                            */
/*----------------------------------------------------------------------------*/
#include <config.h>

#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_min.h>

#include "min.h"

#define REL_ERR_VAL   1.0e-06
#define ABS_ERR_VAL   1.0e-10
#define GOLDEN_MEAN   0.3819660112501052	/* (3 - sqrt(5))/2 */
#define GOLDEN_RATIO  1.6180339887498950	/* (1 + sqrt(5))/2 */

#define DEBUG_PRINTF(x) /* do nothing */

typedef struct
{
  double step_size, stored_step, prev_stored_step;
  double x_prev_small, f_prev_small, x_small, f_small;
  unsigned int num_iter;
}
quad_golden_state_t;

static int
quad_golden_init (void *vstate, gsl_function * f, double x_minimum,
		  double f_minimum, double x_lower, double f_lower,
		  double x_upper, double f_upper)
{
  quad_golden_state_t *state = (quad_golden_state_t *) vstate;

  /* For the original behavior, the first value for x_minimum_minimum
     passed in by the user should be a golden section step but we
     don't enforce this here. */

  state->x_prev_small = x_minimum;
  state->x_small = x_minimum;

  state->f_prev_small = f_minimum;
  state->f_small = f_minimum;

  state->step_size = 0.0;
  state->stored_step = 0.0;
  state->prev_stored_step = 0.0;
  state->num_iter = 0;

  x_lower = 0 ;  /* avoid warnings about unused variables */
  x_upper = 0 ;
  f_lower = 0 ;
  f_upper = 0 ;
  f = 0;

  return GSL_SUCCESS;
}

static int
quad_golden_iterate (void *vstate, gsl_function * f, double *x_minimum,
		     double *f_minimum, double *x_lower, double *f_lower,
		     double *x_upper, double *f_upper)
{
  quad_golden_state_t *state = (quad_golden_state_t *) vstate;

  const double x_m = *x_minimum;
  const double f_m = *f_minimum;

  const double x_l = *x_lower;
  const double x_u = *x_upper;

  const double x_small = state->x_small;
  const double f_small = state->f_small;

  const double x_prev_small = state->x_prev_small;
  const double f_prev_small = state->f_prev_small;

  double stored_step = state->stored_step; /* update on exit */
  double prev_stored_step = state->prev_stored_step; /* update on exit */
  double step_size = state->step_size; /* update on exit */

  double quad_step_size = prev_stored_step;

  double x_trial;
  double x_eval, f_eval;

  double x_midpoint = 0.5 * (x_l + x_u);
  double tol = REL_ERR_VAL * fabs (x_m) + ABS_ERR_VAL; /* total error tolerance */

  if (fabs (stored_step) - tol > -2.0 * GSL_DBL_EPSILON)
    {
      /* Fit quadratic */
      double c3 = (x_m - x_small) * (f_m - f_prev_small);
      double c2 = (x_m - x_prev_small) * (f_m - f_small);
      double c1 = (x_m - x_prev_small) * c2 - (x_m - x_small) * c3;

      c2 = 2.0 * (c2 - c3);

      if (fabs (c2) > GSL_DBL_EPSILON)	/* if( c2 != 0 ) */
	{
	  if (c2 > 0.0)
	    c1 = -c1;

	  c2 = fabs (c2);

	  quad_step_size = c1 / c2;
	}
      else
	{
	  /* Handle case where c2 ~=~ 0  */
	  /* Insure that the line search will NOT take a quadratic
	     interpolation step in this iteration */
	  quad_step_size = stored_step;
	}

      prev_stored_step = stored_step;
      stored_step = step_size;
    }

  x_trial = x_m + quad_step_size;

  if (fabs (quad_step_size) < fabs (0.5 * prev_stored_step) && x_trial > x_l && x_trial < x_u)
    {
      /* Take quadratic interpolation step */
      step_size = quad_step_size;

      /* Do not evaluate function too close to x_l or x_u */
      if ((x_trial - x_l) < 2.0 * tol || (x_u - x_trial) < 2.0 * tol)
        {
          step_size = (x_midpoint >= x_m ? +1.0 : -1.0) * fabs(tol);
        }

      DEBUG_PRINTF(("quadratic step: %g\n", step_size));
    }
  else if ((x_small != x_prev_small && x_small < x_m && x_prev_small < x_m) ||
           (x_small != x_prev_small && x_small > x_m && x_prev_small > x_m))
    {
      /* Take safeguarded function comparison step */
      double outside_interval, inside_interval;

      if (x_small < x_m)
	{
	  outside_interval = x_l - x_m;
	  inside_interval = x_u - x_m;
	}
      else
	{
	  outside_interval = x_u - x_m;
	  inside_interval = x_l - x_m;
	}

      if (fabs (inside_interval) <= tol)
	{
          /* Swap inside and outside intervals */
          double tmp = outside_interval;
	  outside_interval = inside_interval;
	  inside_interval = tmp;
	}

      {
        double step = inside_interval;
        double scale_factor;

        if (fabs (outside_interval) < fabs (inside_interval))
          {
            scale_factor = 0.5 * sqrt (-outside_interval / inside_interval);
          }
        else
          {
            scale_factor = (5.0 / 11.0) * (0.1 - inside_interval / outside_interval);
          }

        state->stored_step = step;
        step_size = scale_factor * step;
      }

      DEBUG_PRINTF(("safeguard step: %g\n", step_size));
    }
  else
    {
      /* Take golden section step */
      double step;

      if (x_m < x_midpoint)
        {
          step = x_u - x_m;
        }
      else
        {
          step = x_l - x_m;
        }

      state->stored_step = step;
      step_size = GOLDEN_MEAN * step;

      DEBUG_PRINTF(("golden step: %g\n", step_size));
    }

  /* Do not evaluate function too close to x_minimum */
  if (fabs (step_size) > tol)
    {
      x_eval = x_m + step_size;
    }
  else
    {
      x_eval = x_m + (step_size >= 0 ? +1.0 : -1.0) * fabs(tol);
    }

  /* Evaluate function at the new point x_eval */
  SAFE_FUNC_CALL(f, x_eval, &f_eval);

  /* Update {x,f}_lower, {x,f}_upper, {x,f}_prev_small, {x,f}_small, and {x,f}_minimum */
  if (f_eval <= f_m)
    {
      if (x_eval < x_m)
	{
          *x_upper = x_m;
          *f_upper = f_m;
        }
      else
      	{
          *x_lower = x_m;
          *f_upper = f_m;
        }

      state->x_prev_small = x_small;
      state->f_prev_small = f_small;

      state->x_small = x_m;
      state->f_small = f_m;

      *x_minimum = x_eval;
      *f_minimum = f_eval;
    }
  else
    {
      if (x_eval < x_m)
        {
          *x_lower = x_eval;
          *f_lower = f_eval;
        }
      else
        {
          *x_upper = x_eval;
          *f_upper = f_eval;
        }

      if (f_eval <= f_small || fabs (x_small - x_m) < 2.0 * GSL_DBL_EPSILON)
	{
	  state->x_prev_small = x_small;
	  state->f_prev_small = f_small;

	  state->x_small = x_eval;
	  state->f_small = f_eval;
	}
      else if (f_eval <= f_prev_small ||
	       fabs (x_prev_small - x_m) < 2.0 * GSL_DBL_EPSILON ||
	       fabs (x_prev_small - x_small) < 2.0 * GSL_DBL_EPSILON)
	{
	  state->x_prev_small = x_eval;
	  state->f_prev_small = f_eval;
	}
    }

  /* Update stored values for next iteration */

  state->stored_step = stored_step;
  state->prev_stored_step = prev_stored_step;
  state->step_size = step_size;
  state->num_iter++;

  DEBUG_PRINTF(("[%d] Final State: %g  %g  %g\n", state->num_iter, x_l, x_m, x_u));

  return GSL_SUCCESS;
}

static const gsl_min_fminimizer_type quad_golden_type = { "quad-golden",	/* name */
  sizeof (quad_golden_state_t),
  &quad_golden_init,
  &quad_golden_iterate
};

const gsl_min_fminimizer_type *gsl_min_fminimizer_quad_golden =
  &quad_golden_type;


/* See compilation notes in simple_cplx.c*/
#include "cplx.h"                 //gsl_cplx_from_c99; see below.
#include <gsl/gsl_blas.h>         //gsl_blas_ddot
#include <gsl/gsl_complex_math.h> //gsl_complex_mul(_real)

gsl_vector_complex *cvec_dot_gslcplx(gsl_vector_complex *v, gsl_complex x){
    gsl_vector_complex *out = gsl_vector_complex_alloc(v->size);
    for (int i=0; i< v->size; i++)
        gsl_vector_complex_set(out, i,
                            gsl_complex_mul(x, gsl_vector_complex_get(v, i)));
    return out;
}

gsl_vector_complex *vec_dot_gslcplx(gsl_vector *v, gsl_complex x){
    gsl_vector_complex *out = gsl_vector_complex_alloc(v->size);
    for (int i=0; i< v->size; i++)
        gsl_vector_complex_set(out, i,
                            gsl_complex_mul_real(x, gsl_vector_get(v, i)));
    return out;
}

gsl_vector_complex *cvec_dot_c(gsl_vector_complex *v, complex double x){
    return cvec_dot_gslcplx(v, gsl_cplx_from_c99(x));
}

gsl_vector_complex *vec_dot_c(gsl_vector *v, complex double x){
    return vec_dot_gslcplx(v, gsl_cplx_from_c99(x));
}

complex double ddot (complex double x, complex double y){return x*y;}

void gsl_vector_complex_print(gsl_vector_complex *v){
    for (int i=0; i< v->size; i++) {
        gsl_complex x = gsl_vector_complex_get(v, i);
        printf("%4g+%4gi%c", GSL_REAL(x), GSL_IMAG(x), i < v->size-1 ? '\t' : '\n');
    }
}


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

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

int
main(void)
{
  const size_t N = 500;                      /* length of time series */
  const size_t K = 11;                       /* window size */
  gsl_movstat_workspace * w = gsl_movstat_alloc(K);
  gsl_vector *x = gsl_vector_alloc(N);
  gsl_vector *xmean = gsl_vector_alloc(N);
  gsl_vector *xmin = gsl_vector_alloc(N);
  gsl_vector *xmax = gsl_vector_alloc(N);
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  size_t i;

  for (i = 0; i < N; ++i)
    {
      double xi = cos(4.0 * M_PI * i / (double) N);
      double ei = gsl_ran_gaussian(r, 0.1);

      gsl_vector_set(x, i, xi + ei);
    }

  /* compute moving statistics */
  gsl_movstat_mean(GSL_MOVSTAT_END_PADVALUE, x, xmean, w);
  gsl_movstat_minmax(GSL_MOVSTAT_END_PADVALUE, x, xmin, xmax, w);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      printf("%zu %f %f %f %f\n",
             i,
             gsl_vector_get(x, i),
             gsl_vector_get(xmean, i),
             gsl_vector_get(xmin, i),
             gsl_vector_get(xmax, i));
    }

  gsl_vector_free(x);
  gsl_vector_free(xmean);
  gsl_rng_free(r);
  gsl_movstat_free(w);

  return 0;
}


/* integration/qk15.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * 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 <config.h>
#include <gsl/gsl_integration.h>

/* Gauss quadrature weights and kronrod quadrature abscissae and
   weights as evaluated with 80 decimal digit arithmetic by
   L. W. Fullerton, Bell Labs, Nov. 1981. */

static const double xgk[8] =    /* abscissae of the 15-point kronrod rule */
{
  0.991455371120812639206854697526329,
  0.949107912342758524526189684047851,
  0.864864423359769072789712788640926,
  0.741531185599394439863864773280788,
  0.586087235467691130294144838258730,
  0.405845151377397166906606412076961,
  0.207784955007898467600689403773245,
  0.000000000000000000000000000000000
};

/* xgk[1], xgk[3], ... abscissae of the 7-point gauss rule.
   xgk[0], xgk[2], ... abscissae to optimally extend the 7-point gauss rule */

static const double wg[4] =     /* weights of the 7-point gauss rule */
{
  0.129484966168869693270611432679082,
  0.279705391489276667901467771423780,
  0.381830050505118944950369775488975,
  0.417959183673469387755102040816327
};

static const double wgk[8] =    /* weights of the 15-point kronrod rule */
{
  0.022935322010529224963732008058970,
  0.063092092629978553290700663189204,
  0.104790010322250183839876322541518,
  0.140653259715525918745189590510238,
  0.169004726639267902826583426598550,
  0.190350578064785409913256402421014,
  0.204432940075298892414161999234649,
  0.209482141084727828012999174891714
};

void
gsl_integration_qk15 (const gsl_function * f, double a, double b,
      double *result, double *abserr,
      double *resabs, double *resasc)
{
  double fv1[8], fv2[8];
  gsl_integration_qk (8, xgk, wg, wgk, fv1, fv2, f, a, b, result, abserr, resabs, resasc);
}


static char help[] =
   "Newton's method for a two-variable system.  Implements analytical Jacobian.\n"
   "Adds struct to hold parameter.\n";

#include <petsc.h>

typedef struct {
  double  b;
} AppCtx;

extern PetscErrorCode FormFunction(SNES, Vec, Vec, void*);
extern PetscErrorCode FormJacobian(SNES, Vec, Mat, Mat, void*);

//STARTMAIN
int main(int argc,char **argv) {
  SNES   snes;         // nonlinear solver context
  Vec    x,r;          // solution, residual vectors
  Mat    J;
  AppCtx user;
  PetscErrorCode ierr;

  PetscInitialize(&argc,&argv,NULL,help);
  user.b = 2.0;

  ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
  ierr = VecSetSizes(x,PETSC_DECIDE,2); CHKERRQ(ierr);
  ierr = VecSetFromOptions(x); CHKERRQ(ierr);
  ierr = VecDuplicate(x,&r); CHKERRQ(ierr);

  ierr = MatCreate(PETSC_COMM_WORLD,&J); CHKERRQ(ierr);
  ierr = MatSetSizes(J,PETSC_DECIDE,PETSC_DECIDE,2,2); CHKERRQ(ierr);
  ierr = MatSetFromOptions(J); CHKERRQ(ierr);
  ierr = MatSetUp(J); CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
  ierr = SNESSetFunction(snes,r,FormFunction,&user);CHKERRQ(ierr);
  ierr = SNESSetJacobian(snes,J,J,FormJacobian,&user);CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

  ierr = VecSet(x,1.0);CHKERRQ(ierr);
  ierr = SNESSolve(snes,NULL,x);CHKERRQ(ierr);
  ierr = VecView(x,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);

  VecDestroy(&x);  VecDestroy(&r);  SNESDestroy(&snes);  MatDestroy(&J);
  return PetscFinalize();
}
//ENDMAIN

//STARTFORM
PetscErrorCode FormFunction(SNES snes, Vec x, Vec F, void *ctx) {
    PetscErrorCode ierr;
    AppCtx       *user = (AppCtx*)ctx;
    const double b = user->b, *ax;
    double       *aF;

    ierr = VecGetArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF);CHKERRQ(ierr);
    aF[0] = (1.0 / b) * PetscExpReal(b * ax[0]) - ax[1];
    aF[1] = ax[0] * ax[0] + ax[1] * ax[1] - 1.0;
    ierr = VecRestoreArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormJacobian(SNES snes, Vec x, Mat J, Mat P, void *ctx) {
    PetscErrorCode ierr;
    AppCtx       *user = (AppCtx*)ctx;
    const double b = user->b, *ax;
    double       v[4];
    int          row[2] = {0,1}, col[2] = {0,1};

    ierr = VecGetArrayRead(x,&ax); CHKERRQ(ierr);
    v[0] = PetscExpReal(b * ax[0]);  v[1] = -1.0;
    v[2] = 2.0 * ax[0];              v[3] = 2.0 * ax[1];
    ierr = VecRestoreArrayRead(x,&ax); CHKERRQ(ierr);
    ierr = MatSetValues(P,2,row,2,col,v,INSERT_VALUES); CHKERRQ(ierr);
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}
//ENDFORM


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include "linear_algebra.h"

Matrix *new_matrix(int row_num, int col_num)
{
    Matrix *matrix = malloc(sizeof(Matrix));
    if (matrix == NULL) return NULL;
    matrix->data = malloc((unsigned long) (row_num * col_num) * sizeof(double));

    if (matrix->data == NULL)
    {
        perror("Error allocating memory for a matrix");
        exit(EXIT_FAILURE);
    }

    matrix->row_num = row_num;
    matrix->col_num = col_num;

    return matrix;
}

Matrix *new_matrix_from_array(double *array, int row_num, int col_num)
{
   Matrix *matrix = new_matrix(row_num, col_num);
   memcpy(matrix->data, array, (unsigned long)(row_num * col_num) * sizeof(double));
   return matrix;
}

void free_matrix(Matrix *matrix)
{
    free(matrix->data);
    free(matrix);
}

Matrix *add_matrices(Matrix *matrix1, Matrix *matrix2)
{
    if (matrix1->row_num != matrix2->row_num ||
        matrix1->col_num != matrix2->col_num)
    {
        perror("Dimensions of the two matrices do not match");
        exit(EXIT_FAILURE);
    }

    Matrix *result_matrix = new_matrix(matrix1->row_num, matrix1->col_num);

    int i;
    int elements = matrix1->row_num * matrix1->col_num;

    for (i = 0; i < elements; i++)
    {
        result_matrix->data[i] = matrix1->data[i] + matrix2->data[i];
    }

    return result_matrix;
}

Matrix *multiply_matrices(Matrix *matrix1, Matrix *matrix2)
{
    if (matrix1->col_num != matrix2->row_num)
    {
        perror("Incompatible matrix dimensions");
        exit(EXIT_FAILURE);
    }

    Matrix *product = new_matrix(matrix1->row_num, matrix2->col_num);

    int m = matrix1->row_num;
    int n = matrix2->col_num;
    int k = matrix1->col_num;

    // Use high-performace matrix multiplication from BLAS
    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1,
        matrix1->data, k, matrix2->data, n, 0, product->data, n);

    return product;
}

void multiply_vector_by_vector_transpose(Matrix *vector1, Matrix *vector2, double scalar, Matrix *matrix)
{
    if (vector1->col_num != 1 || vector2->col_num != 1)
    {
        perror("Not a vector.");
        exit(EXIT_FAILURE);
    }

    int m = vector1->row_num;
    int n = vector2->row_num;

    cblas_dger(CblasRowMajor, m, n, scalar, vector1->data, 1, vector2->data, 1, matrix->data, n);
}

Matrix *multiply_matrix_with_vector(Matrix *matrix, Matrix *vector, double scalar)
{
    if (matrix->col_num != vector->row_num)
    {
        perror("Incompatible matrix dimensions");
        exit(EXIT_FAILURE);
    }

    if (vector->col_num != 1)
    {
        perror("Not a vector.");
        exit(EXIT_FAILURE);
    }

    Matrix *product = new_matrix(matrix->row_num, 1);

    int m = matrix->row_num;
    int n = matrix->col_num;

    cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, scalar, matrix->data, n,
                vector->data, 1,  0, product->data, 1);

    return product;
}

Matrix *multiply_upper_symmetric_matrix_with_vector(Matrix *matrix, Matrix *vector)
{
    if (matrix->col_num != matrix->row_num)
    {
        perror("Matrix is not square");
        exit(EXIT_FAILURE);
    }

    if (matrix->col_num != vector->row_num)
    {
        perror("Incompatible matrix dimensions");
        exit(EXIT_FAILURE);
    }

    if (vector->col_num != 1)
    {
        perror("Not a vector.");
        exit(EXIT_FAILURE);
    }

    Matrix *product = new_matrix(matrix->row_num, 1);

    int n = matrix->col_num;

    cblas_dsymv(CblasRowMajor, CblasUpper, n, 1, matrix->data,
                n, vector->data, 1, 0, product->data, 1);

    return product;
}

void multiply_matrix_with_a_number(Matrix *matrix, double number)
{
    int i;
    int n = matrix->col_num;
    int row_num = matrix->row_num;

    for (i = 0; i < row_num; i++)
    {
        cblas_dscal(n, number, matrix->data + i * n, 1);
    }
}

Matrix *transpose_matrix(Matrix *matrix)
{
    Matrix *product = new_matrix(matrix->col_num, matrix->row_num);

    int i, j;

    for (i = 0; i < matrix->col_num; i++)
    {
        for (j = 0; j < matrix->row_num; j++)
        {
            product->data[i * matrix->row_num + j] = matrix->data[j * matrix->col_num + i];
        }
    }

    return product;
}

double dot_product(Matrix *matrix1, Matrix *matrix2)
{
    if (matrix1->row_num != matrix2->row_num ||
        matrix1->col_num != 1 || matrix2->col_num != 1)
    {
        perror("Matrices are not n by 1.");
        exit(EXIT_FAILURE);
    }

    int i;
    double result = 0;

    for (i = 0; i < matrix1->row_num; i++)
    {
        result += matrix1->data[i] * matrix2->data[i];
    }

    return result;
}

double norm(Matrix *matrix)
{
    return sqrt(dot_product(matrix, matrix));
}

Matrix *gramian(Matrix *matrix)
{
    Matrix *product = new_matrix(matrix->col_num, matrix->col_num);

    int n = matrix->col_num;
    int k = matrix->row_num;

    cblas_dsyrk(CblasRowMajor, CblasUpper, CblasTrans,
		 n, k, 1, matrix->data, n, 0, product->data, n);

    return product;
}


/***************************************************************************************
 *  Multivariate Normal density function and random number generator
 *  Multivariate Student t density function and random number generator
 *  Wishart random number generator
 *  Using GSL -> www.gnu.org/software/gsl
 *
 *  Copyright (C) 2006  Ralph dos Santos Silva
 *
 *  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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  AUTHOR
 *     Ralph dos Santos Silva,  [EMAIL PROTECTED]
 *     March, 2006
***************************************************************************************/
#include <math.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include "../hmm/adkGSL.h"
#include "mvnorm_gsl.h"
/*****************************************************************************************************************/
/*****************************************************************************************************************/
int rmvnorm(const gsl_rng *r, const int n, const gsl_vector *mean, const gsl_matrix *var, gsl_vector *result){
/* multivariate normal distribution random number generator */
/*
*	n	dimension of the random vetor
*	mean	vector of means of size n
*	var	variance matrix of dimension n x n
*	result	output variable with a sigle random vector normal distribution generation
*/
int k;
gsl_matrix *work = gsl_matrix_alloc(n,n);

gsl_matrix_memcpy(work,var);
gsl_linalg_cholesky_decomp(work);

for(k=0; k<n; k++)
	gsl_vector_set( result, k, gsl_ran_ugaussian(r) );

gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, work, result);
gsl_vector_add(result,mean);

gsl_matrix_free(work);

return 0;
}

/*****************************************************************************************************************/
/*****************************************************************************************************************/
int rmvnorm_prealloc(const gsl_rng *r, const int n, const gsl_vector *mean, const gsl_matrix *var, gsl_matrix *work, gsl_vector *result){
/* multivariate normal distribution random number generator */
/*
*	n	dimension of the random vetor
*	mean	vector of means of size n
*	var	variance matrix of dimension n x n
*	result	output variable with a sigle random vector normal distribution generation
*/
int k;

gsl_matrix_memcpy(work,var);
gsl_linalg_cholesky_decomp(work);

for(k=0; k<n; k++)
	gsl_vector_set( result, k, gsl_ran_ugaussian(r) );

gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, work, result);
gsl_vector_add(result,mean);

return 0;
}

/*****************************************************************************************************************/
/*****************************************************************************************************************/
double dmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var){
/* multivariate normal density function    */
/*
*	n	dimension of the random vetor
*	mean	vector of means of size n
*	var	variance matrix of dimension n x n
*/
int s;
double ax,ay;
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
           *winv = gsl_matrix_alloc(n,n);
gsl_permutation *p = gsl_permutation_alloc(n);

gsl_matrix_memcpy( work, var );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );

xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, mean );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);
ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );

return ay;
}

/*****************************************************************************************************************/
/*****************************************************************************************************************/
int rmvt(const gsl_rng *r, const int n, const gsl_vector *location, const gsl_matrix *scale, const int dof, gsl_vector *result){
/* multivariate Student t distribution random number generator */
/*
*	n	 dimension of the random vetor
*	location vector of locations of size n
*	scale	 scale matrix of dimension n x n
*	dof	 degrees of freedom
*	result	 output variable with a single random vector normal distribution generation
*/
int k;
gsl_matrix *work = gsl_matrix_alloc(n,n);
double ax = 0.5*dof;

ax = gsl_ran_gamma(r,ax,(1/ax));     /* gamma distribution */

gsl_matrix_memcpy(work,scale);
gsl_matrix_scale(work,(1/ax));       /* scaling the matrix */
gsl_linalg_cholesky_decomp(work);

for(k=0; k<n; k++)
	gsl_vector_set( result, k, gsl_ran_ugaussian(r) );

gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, work, result);
gsl_vector_add(result, location);

gsl_matrix_free(work);

return 0;
}

/*****************************************************************************************************************/
/*****************************************************************************************************************/
double dmvt(const int n, const gsl_vector *x, const gsl_vector *location, const gsl_matrix *scale, const int dof){
/* multivariate Student t density function */
/*
*	n	 dimension of the random vetor
*	location vector of locations of size n
*	scale	 scale matrix of dimension n x n
*	dof	 degrees of freedom
*/
int s;
double ax,ay,az=0.5*(dof + n);
gsl_vector *ym, *xm;
gsl_matrix *work = gsl_matrix_alloc(n,n),
           *winv = gsl_matrix_alloc(n,n);
gsl_permutation *p = gsl_permutation_alloc(n);

gsl_matrix_memcpy( work, scale );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_matrix_free( work );
gsl_permutation_free( p );

xm = gsl_vector_alloc(n);
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, location );
ym = gsl_vector_alloc(n);
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_matrix_free( winv );
gsl_blas_ddot( xm, ym, &ay);
gsl_vector_free(xm);
gsl_vector_free(ym);

ay = pow((1+ay/dof),-az)*gsl_sf_gamma(az)/(gsl_sf_gamma(0.5*dof)*sqrt( pow((dof*M_PI),n)*ax ));

return ay;
}
/*****************************************************************************************************************/
/*****************************************************************************************************************/
int rwishart(const gsl_rng *r, const int n, const int dof, const gsl_matrix *scale, gsl_matrix *work, gsl_matrix *output){
/* Wishart distribution random number generator */
/*
*	n	 gives the dimension of the random matrix
*	dof	 degrees of freedom
*	scale	 scale matrix of dimension n x n
*	result	 output variable with a single random matrix Wishart distribution generation
*/
int k,l;

for(k=0; k<scale->size1; k++){
	gsl_matrix_set( work, k, k, sqrt( gsl_ran_chisq( r, (dof-k) ) ) );
	for(l=0; l<k; l++){
		gsl_matrix_set( work, k, l, gsl_ran_ugaussian(r) );
	}
}
gsl_matrix_memcpy(output,scale);
gsl_linalg_cholesky_decomp(output);
gsl_blas_dtrmm(CblasLeft,CblasLower,CblasNoTrans,CblasNonUnit,1.0,output,work);
gsl_blas_dsyrk(CblasUpper,CblasNoTrans,1.0,work,0.0,output);
return 0;
}

// ADK additions from here on

/*****************************************************************************************************************/
/*****************************************************************************************************************/
double dmvnorm_prealloc(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var, gsl_matrix *work,
	 gsl_matrix *winv,  gsl_vector *ym,  gsl_vector *xm, gsl_permutation *p){
/* multivariate normal density function    */
/*
*	n	dimension of the random vetor
*	mean	vector of means of size n
*	var	variance matrix of dimension n x n
*
*   added this prealloc version to cut down on memory calls
*/

int s;
double ax,ay;

gsl_matrix_memcpy( work, var );
gsl_linalg_LU_decomp( work, p, &s );
gsl_linalg_LU_invert( work, p, winv );
ax = gsl_linalg_LU_det( work, s );
gsl_vector_memcpy( xm, x);
gsl_vector_sub( xm, mean );
gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym);
gsl_blas_ddot( xm, ym, &ay);
ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax );

return ay;
}

/*****************************************************************************************************************/
/*****************************************************************************************************************/
double dInverseWishart(const gsl_matrix *x, const gsl_matrix *sigma, const double dof, gsl_matrix *work,
	 gsl_matrix *winv,  gsl_permutation *p){

	double denom, num;
	int s, d, i;
	double detSig,detX,trace;

	//get det of both matrices, take inverse of X
	gsl_matrix_memcpy( work, sigma );
	gsl_linalg_LU_decomp( work, p, &s );
	detSig = gsl_linalg_LU_det( work, s );
	gsl_matrix_memcpy( work, x );
	gsl_linalg_LU_decomp( work, p, &s );
	detX = gsl_linalg_LU_det( work, s );
	gsl_linalg_LU_invert( work, p, winv );
	//get product of gamma's for denominator; calc denom
	d = x->size1;
	denom = 1.0;
	denom *= pow(2,(d*dof/2.0)) * mv_gamma_func(dof/2.0, d);

	//multiply x by inverse of sigma and take trace
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,1.0, sigma, winv,0.0, work);
	trace = gsl_matrix_trace(work);
	num =  pow(detSig, dof / 2.0) * pow(detX , (-dof-d-1)/ 2.0) * exp(-0.5 * trace);
	return(num/denom);
}

/*****************************************************************************************************************/
//rwishartGelman-- uses alg described in Gelman et al appendix
void rwishartGelman(const gsl_rng *r, const int n, const int dof, const gsl_matrix *scale,
	gsl_matrix *work,gsl_matrix *work2, gsl_vector *mean,gsl_vector *xm, gsl_matrix *output){
/* Wishart distribution random number generator */
/*
*	n	 gives the dimension of the random matrix
*	dof	 degrees of freedom
*	scale	 scale matrix of dimension n x n
*	result	 output variable with a single random matrix Wishart distribution generation
*/
	int i;

	gsl_vector_set_all(mean,0.0);
	gsl_matrix_set_all(output,0.0);
	for(i = 0; i < dof; i++){
		gsl_vector_set_all(xm,0);
		rmvnorm_prealloc(r, n, mean, scale, work, xm);
		gsl_vector_outer_product(xm,xm,work2);
		gsl_matrix_add(output,work2);
	}
}

//rwishartOdellFeiveson-- uses alg of Odell and Feiveson
void rwishartOdellFeiveson(const gsl_rng *r, const int n, const int dof, const gsl_matrix *scale,
	gsl_matrix *work,gsl_matrix *work2, gsl_vector *mean,gsl_vector *xm, gsl_matrix *output){
/* Wishart distribution random number generator */
/*
*	n	 gives the dimension of the random matrix
*	dof	 degrees of freedom
*	scale	 scale matrix of dimension n x n
*	result	 output variable with a single random matrix Wishart distribution generation
*/
	int i,j, k;
	double sum, tmp;

	gsl_vector_set_all(mean,0.0);
	gsl_matrix_set_all(output,0.0);

	for(j=0;j<n;j++){
		gsl_vector_set(mean,j,gsl_ran_chisq(r,dof-j));
		for(i=0;i<j;i++){
			gsl_matrix_set(work,i,j,gsl_ran_ugaussian(r));
		}
	}

	//set up B matrix in work2
	gsl_matrix_set(work2,0,0,gsl_vector_get(mean,0));
	for(j=0;j<n;j++){
		for(i=0;i<j;i++){
			if(i==0){
				tmp = sqrt(gsl_vector_get(mean,j)) * gsl_matrix_get(work,0,j);
				gsl_matrix_set(work2,0,j,tmp);
			}
			else{
				tmp = sqrt(gsl_vector_get(mean,i)) * gsl_matrix_get(work,i,j);
				for(k=0;k<i;k++){
					tmp += gsl_matrix_get(work,k,i) * gsl_matrix_get(work,k,j);
				}
				gsl_matrix_set(work2,i,j,tmp);
			}
		}
		tmp = gsl_vector_get(mean,j);
		for(k=0;k<j;k++){
			tmp += gsl_matrix_get(work,k,j) * gsl_matrix_get(work,k,j);
		}
		gsl_matrix_set(work2,j,j,tmp);
	}
	gsl_matrix_memcpy(output,scale);
	gsl_linalg_cholesky_decomp(output);
	gsl_blas_dtrmm(CblasLeft,CblasLower,CblasNoTrans,CblasNonUnit,1.0,output,work2);
	gsl_blas_dtrmm(CblasRight,CblasUpper,CblasNoTrans,CblasNonUnit,1.0,work2,output);

}

//mv_gamma_func -- returns the multivariate gamma function
double mv_gamma_func(double a, double d){
	double val = 1.0;
	int i;

	for(i=1;i<=d;i++){
		 val *= gsl_sf_gamma(a- (0.5 * (i-1)));
		}
	val *=  pow(M_PI,(d*(d-1)/4.0));
	return(val);
}


#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>

// author:    Andrew Liew <liew@arch.ethz.ch>
// copyright: Copyright 2018, BLOCK Research Group - ETH Zurich
// license:   MIT License
// email:     liew@arch.ethz.ch

int i;
int j;

// ==========================================================================================================
// One vector
// ==========================================================================================================

double length_vector(gsl_vector *u)
{
    double L = gsl_blas_dnrm2(u);

    return L;
}

double length_vector_squared(gsl_vector *u)
{
    double a = 0.;

    for (i = 0; i < 3; i++)
    {
        a += gsl_pow_2(gsl_vector_get(u, i));
    }

    return a;
}

void normalize_vector(gsl_vector *u)
{
    gsl_vector_scale(u, 1./length_vector(u));
}

void scale_vector(gsl_vector *u, double a)
{
    gsl_vector_scale(u, a);
}

void vector_from_pointer(double *ptr, gsl_vector *w)
{
    gsl_vector_set(w, 0, *(ptr + 0));
    gsl_vector_set(w, 1, *(ptr + 1));
    gsl_vector_set(w, 2, *(ptr + 2));
}

// ==========================================================================================================
// Two vectors
// ==========================================================================================================

void add_vectors(const gsl_vector *u, const gsl_vector *v, gsl_vector *w)
{
    gsl_vector_memcpy(w, u);
    gsl_vector_add(w, v);
}

void cross_vectors(const gsl_vector *u, const gsl_vector *v, gsl_vector *w)
{
    double w1 = gsl_vector_get(u, 1) * gsl_vector_get(v, 2) - gsl_vector_get(u, 2) * gsl_vector_get(v, 1);
    double w2 = gsl_vector_get(u, 2) * gsl_vector_get(v, 0) - gsl_vector_get(u, 0) * gsl_vector_get(v, 2);
    double w3 = gsl_vector_get(u, 0) * gsl_vector_get(v, 1) - gsl_vector_get(u, 1) * gsl_vector_get(v, 0);

    gsl_vector_set(w, 0, w1);
    gsl_vector_set(w, 1, w2);
    gsl_vector_set(w, 2, w3);
}

double dot_vectors(const gsl_vector *u, const gsl_vector *v)
{
    double a = 0.;

    for (i = 0; i < 3; i++)
    {
        a += gsl_vector_get(u, i) * gsl_vector_get(v, i);
    }

    return a;
}

void subtract_vectors(const gsl_vector *u, const gsl_vector *v, gsl_vector *w)
{
    gsl_vector_memcpy(w, u);
    gsl_vector_sub(w, v);
}