text:
//
//  syrk.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/4/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _syrk_h
#define _syrk_h

/// @file syrk.h Performs matrix-matrix multiplication operations resulting in symmetric matrices.

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

namespace LATL
{
   /// @brief Performs multiplcation of a real matrix with its transpose.
   ///
   /// For real matrix A, real symmetric matrix C, and real scalars alpha and beta
   ///
   ///        C := alpha*A*A'+beta*C  or  C := alpha*A'*A+beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether the upper or lower triangular part of the symmetric matrix C
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then C is upper triangular,
   ///        if uplo = 'L' or 'l' then C is lower triangular.
   /// @param trans Specifies the operation to be perfomed as follows:
   ///
   ///        if trans = 'N' or 'n' then C := alpha*A*A'+beta*C
   ///        if trans = 'T' or 't' then C := alpha*A'*A+beta*C
   ///        if trans = 'C' or 'c' then C := alpha*A'*A+beta*C
   /// @param n Specifies the order of the symmetric matrix C.  n>=0
   /// @param k Specifies the other dimension of the real matrix A (see below). k>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real matrix.
   ///
   ///        if trans = 'N' or 'n' then A is n-by-k
   ///        if trans = 'T' or 't' then A is k-by-n
   ///        if trans = 'C' or 'c' then A is k-by-n
   /// @param ldA Column length of the matrix A. If trans = 'N' or 'n' ldA>=n, otherwise ldA>=k.
   /// @param beta Real scalar.
   /// @param C Pointer to real symmetric n-by-n matrix C.
   /// Only the upper or lower triangular part of C is referenced, depending on the value of uplo above.
   /// @param ldC Column length of the matrix C.  ldC>=n
   /// @ingroup BLAS

   template <typename real_t>
   int SYRK(char uplo, char trans, int_t n, int_t k, real_t alpha, real_t *A, int_t ldA, real_t beta, real_t *C, int_t ldC)
   {
      using std::toupper;

      const real_t zero(0.0);
      const real_t one(1.0);
      int_t i,j,l;
      real_t *a,*c,*at;
      real_t t;

      uplo=toupper(uplo);
      trans=toupper(trans);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;
      else if((n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               c+=ldC;
            }

         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               c+=ldC;
            }
         }
      }
      else if(trans=='N')
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=0;i<=j;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=j;i<n;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            c=C;
            at=A;
            for(j=0;j<n;j++)
            {
               a=A;
               for(i=0;i<=j;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
         else
         {
            at=A;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A+j*ldA;
               for(i=j;i<n;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
      }
      return 0;
   }

   /// @brief Performs multiplcation of a complex matrix with its transpose.
   ///
   /// For complex matrix A, complex symmetric matrix C, and complex scalars alpha and beta
   ///
   ///        C := alpha*A*A.'+beta*C  or  C := alpha*A.'*A+beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether the upper or lower triangular part of the symmetric matrix C
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then C is upper triangular,
   ///        if uplo = 'L' or 'l' then C is lower triangular.
   /// @param trans Specifies the operation to be perfomed as follows:
   ///
   ///        if trans = 'N' or 'n' then C := alpha*A*A.'+beta*C
   ///        if trans = 'T' or 't' then C := alpha*A.'*A+beta*C
   /// @param n Specifies the order of the complex symmetric matrix C.  n>=0
   /// @param k Specifies the other dimension of the complex matrix A (see below). k>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex matrix.
   ///
   ///        if trans = 'N' or 'n' then A is n-by-k
   ///        if trans = 'T' or 't' then A is k-by-n
   /// @param ldA Column length of the matrix A. If trans = 'N' or 'n' ldA>=n, otherwise ldA>=k.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex symmetric n-by-n matrix C.
   /// Only the upper or lower triangular part of C is referenced, depending on the value of uplo above.
   /// @param ldC Column length of the matrix C.  ldC>=n
   /// @ingroup BLAS

   template <typename real_t>
   int SYRK(char uplo, char trans, int_t n, int_t k, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::toupper;

      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,l;
      complex<real_t> *a,*c,*at;
      complex<real_t> t;

      uplo=toupper(uplo);
      trans=toupper(trans);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;
      else if((n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               c+=ldC;
            }

         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               c+=ldC;
            }
         }
      }
      else if(trans=='N')
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=0;i<=j;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               a=A;
               for(l=0;l<k;l++)
               {
                  t=alpha*a[j];
                  for(i=j;i<n;i++)
                     c[i]+=t*a[i];
                  a+=ldA;
               }
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            c=C;
            at=A;
            for(j=0;j<n;j++)
            {
               a=A;
               for(i=0;i<=j;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
         else
         {
            at=A;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A+j*ldA;
               for(i=j;i<n;i++)
               {
                  t=zero;
                  for(l=0;l<k;l++)
                     t+=a[l]*at[l];
                  c[i]=alpha*t+beta*c[i];
                  a+=ldA;
               }
               at+=ldA;
               c+=ldC;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int SYRK<float>(char uplo, char trans, int_t n, int_t k, float alpha, float *A, int_t ldA, float beta, float *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_ssyrk(CblasColMajor,Uplo,Trans,n,k,alpha,A,ldA,beta,C,ldC);

      return 0;

   }

   template <> int SYRK<double>(char uplo, char trans, int_t n, int_t k, double alpha, double *A, int_t ldA, double beta, double *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_dsyrk(CblasColMajor,Uplo,Trans,n,k,alpha,A,ldA,beta,C,ldC);

      return 0;

   }

   template <> int SYRK<float>(char uplo, char trans, int_t n, int_t k, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> beta, complex<float> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_csyrk(CblasColMajor,Uplo,Trans,n,k,&alpha,A,ldA,&beta,C,ldC);

      return 0;

   }

   template <> int SYRK<double>(char uplo, char trans, int_t n, int_t k, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> beta, complex<double> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldC<n)
         return -10;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_zsyrk(CblasColMajor,Uplo,Trans,n,k,&alpha,A,ldA,&beta,C,ldC);

      return 0;

   }

#endif
}

#endif

ext: h

max_stars_repo_name: langou/latl

max_issues_repo_name: langou/latl

max_forks_repo_name: langou/latl


text:
/**
 * @file convolution.h
 * @brief Block convolution realization of an LTI system with the FFT.Time delay estimation
 * @author John McDonough
 */

#ifndef CONVOLUTION_H
#define CONVOLUTION_H

#include <stdio.h>
#include <assert.h>

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include "common/jexception.h"

#include "stream/stream.h"
#include "feature/feature.h"

// ----- definition for class `OverlapAdd' -----
//
class OverlapAdd : public VectorFloatFeatureStream {
 public:
  OverlapAdd(VectorFloatFeatureStreamPtr& samp,
             const gsl_vector* impulseResponse = NULL, unsigned fftLen = 0,
             const String& nm = "OverlapAdd");

  ~OverlapAdd();

  virtual const gsl_vector_float* next(int frame_no = -5);

  virtual void reset();

 private:
  void set_impulse_response_(const gsl_vector* impulseResponse);
  unsigned check_fftLen_(unsigned sectionLen, unsigned irLen, unsigned fftLen);

  const VectorFloatFeatureStreamPtr			samp_;
  const unsigned					L_;
  const unsigned					P_;
  const unsigned					N_;
  const unsigned					N2_;

  double*						section_;
  gsl_vector_complex*					frequencyResponse_;
  gsl_vector_float*					buffer_;
};

typedef Inherit<OverlapAdd, VectorFloatFeatureStreamPtr> OverlapAddPtr;

// ----- definition for class `OverlapSave' -----
//
class OverlapSave : public VectorFloatFeatureStream {
 public:
  OverlapSave(VectorFloatFeatureStreamPtr& samp,
              const gsl_vector* impulseResponse = NULL, const String& nm = "OverlapSave");

  ~OverlapSave();

  virtual const gsl_vector_float* next(int frame_no = -5);

  virtual void reset();

  void update(const gsl_vector_complex* delta);

 private:
  void set_impulse_response_(const gsl_vector* impulseResponse);
  unsigned check_output_size_(unsigned irLen, unsigned sampLen);
  unsigned check_L_(unsigned irLen, unsigned sampLen);

  const VectorFloatFeatureStreamPtr			samp_;
  const unsigned					L_;
  const unsigned					L2_;
  const unsigned					P_;

  double*						section_;
  gsl_vector_complex*					frequencyResponse_;
};

typedef Inherit<OverlapSave, VectorFloatFeatureStreamPtr> OverlapSavePtr;

#endif // CONVOLUTION_H


text:
#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;
}

ext: c

max_stars_repo_name: ielomariala/Hex-Game

max_issues_repo_name: Brian-ning/HMNE

max_forks_repo_name: Brian-ning/HMNE


text:
#ifndef FUNCTIONS_H
#define FUNCTIONS_H

#include <vector>
#include <string>
#include <fstream>

#include <gsl/gsl_matrix.h>

#include "structures.h"

double rad2deg(double rad);
double deg2rad(double deg);
double vect_len(const vect &v);
void norm(vect &vr);

double double_dot_prod(const gsl_matrix* m1, const gsl_matrix* m2);
double calc_evM(const gsl_matrix* def);
double calc_internal_energy(const gsl_matrix* shear, const slip_system_matrix *smatrix, grain *g);
double calc_energy(const gsl_matrix* slpsysmat, const gsl_matrix* def_l, const slip_system_matrix *smatrix, grain *g, bool falg=false);
void cleanup_energy();

double update_rot_matrix(const std::vector<int>& comb, const gsl_matrix* shaermin, const std::vector<slip_system*>& systems, const gsl_matrix* uk1, const gsl_matrix* uk2, gsl_matrix* rmat, int def_step);

gsl_matrix* get_rot_matrix(const grain *g, bool free_m=false);
grain* grain_from_rot_matrix(gsl_matrix *rot_mat);

void print_matrix(const gsl_matrix *m);

void load_settings(double *max_def, double *def_step, gsl_matrix *deformation, gsl_matrix *k1, gsl_matrix *k2, const std::string &settings_file_name);
void load_grains(std::vector<grain*> &grains, const std::string &grains_file_name);
void load_slip_systems(std::vector<slip_system*> &slip_systems, const std::string &slip_system_file, int max_def);

std::vector<grain*>* prepare_rot_matrices(const std::string &grains_file_name);
void fill_grain_crss(std::vector<grain*> *grains, const std::vector<slip_system*> &slip_systems);
void prepare_slip_systems_matrices(std::vector<slip_system_matrix*> &matrices, std::vector<slip_system*> &slip_systems);

void convert2U(const gsl_matrix* mat, gsl_matrix* umat);
void calc_handler(handler &h, const gsl_matrix *def, const gsl_matrix *rm);
void get_uhandler(U_handler &uh, const handler &h, int s=2);

void write_grains(const std::vector<grain*> &grinas, std::ofstream &file, const std::string &delimiter);
void export_grains_to_file(const std::vector<grain*> &grains, const std::string &export_file, const std::string &delimiter);
void export_taylor_factor_to_file(const std::vector<double> &total_tf, const std::string& filename);
void write_slip_system_activity(const std::vector<slip_system*> &slip_systems, std::ofstream &export_file, int grains_num, double eVM);
void export_slip_system_activity_to_file(const std::vector<slip_system*> &slip_systems, const std::string& filename, int grains_num, double eVM);

void free_grains(std::vector<grain*> &grains);
void free_slip_systems(std::vector<slip_system*> &slip_systems);
void free_rotation_matrices(std::vector<gsl_matrix*> rot_matrices);
void free_slip_systems_matrices(std::vector<slip_system_matrix*> &matrices);

#endif	//FUNCTIONS_H

max_stars_repo_name: bainit/hextexsim

max_issues_repo_name: bainit/hextexsim

max_forks_repo_name: bainit/hextexsim


text:
/* AUTORIGHTS
Copyright (C) 2007 Princeton University

This file is part of Ferret Toolkit.

Ferret Toolkit 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 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 <math.h>
#ifdef _OPENMP
#include <omp.h>
#endif
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <cass.h>
#include "LSH.h"
#include "local.h"

#define ERR	1e-4

struct local_params
{
	double a;
	double b;
	double W;
	int M;
	int L;
	int K;
};

static inline double p_col_helper (double x)
{
	double result;
	result = 2.0*gsl_cdf_ugaussian_P(x) - 1.0;
	result += sqrt(2.0/M_PI) * (exp(-x*x/2.0)-1.0)/x;
	return result;
}

static inline double p_col (double x, double W, int M, int L)
{
	return 1.0 - pow(1.0 - pow(p_col_helper(W/x), M), L);
}

static int fun (const gsl_vector *x, void *params, gsl_vector *f)
{
	double W = ((struct local_params *)params)->W;
	int M = ((struct local_params *)params)->M;
	int L = ((struct local_params *)params)->L;
	int K = ((struct local_params *)params)->K;
	double fa = ((struct local_params *)params)->a;
	double fb = ((struct local_params *)params)->b;
	double sxx, sxy, sx, sy, k, d;
	double a, b;
	int n;

	double alpha = gsl_vector_get(x, 0);
	double beta = gsl_vector_get(x, 1);

	sxx = sxy = sx = sy = 0;

	n = 0;
	k = 0;
	while ((k < K) && (n < 1000))
	{
		double lk, ld;
		n++;
		d = alpha * pow(n, beta);
		k += p_col(sqrt(d), W, M, L);
		lk = log(k);
		ld = log(d);
		sx += lk;
		sy += ld;
		sxx += lk * lk;
		sxy += lk * ld;
	}

	least_squares(&a, &b, n, sxx, sxy, sx, sy);

	a = exp(a);

	gsl_vector_set(f, 0, a - fa);
	gsl_vector_set(f, 1, b - fb);
	return GSL_SUCCESS;
}

int print_state (size_t iter, gsl_multiroot_fsolver *s)
{
	fprintf(stderr, "iter = %3zu x = %.3f %.3f "
		"f(x) = %.3e %.3e\n",
		iter,
		gsl_vector_get(s->x, 0),
		gsl_vector_get(s->x, 1),
		gsl_vector_get(s->f, 0),
		gsl_vector_get(s->f, 1));
	return 0;
}

int localdim (double a, double b, double *alpha, double *beta, double W, int M, int L, int K)
{
	const gsl_multiroot_fsolver_type *T = NULL;
	gsl_multiroot_fsolver *s = NULL;

	int status;
	size_t iter = 0;

	struct local_params params = {.a = a, .b = b, .W = W, .M = M, .L = L, .K = K};
	gsl_multiroot_function f = {&fun, 2, &params};

	gsl_vector *x = gsl_vector_alloc(2);
	gsl_vector_set(x, 0, a);
	gsl_vector_set(x, 1, b);

	if (s == NULL)
	{
		T = gsl_multiroot_fsolver_hybrids;
		s = gsl_multiroot_fsolver_alloc(T, 2);
	}

	gsl_multiroot_fsolver_set(s, &f, x);

	do
	{
		status = gsl_multiroot_fsolver_iterate(s);

//		print_state(iter, s);

		if (status) break;

		status = gsl_multiroot_test_residual(s->f, ERR);
		iter++;
	} while ((status == GSL_CONTINUE) && (iter < 1000));

//	fprintf(stderr, "status = %s\n", gsl_strerror(status));
	*alpha = gsl_vector_get(s->x, 0);
	*beta = gsl_vector_get(s->x, 1);

	gsl_multiroot_fsolver_free(s);
	gsl_vector_free(x);
	return 0;
}

int LSH_est_init (LSH_est_t *est, int a_step, double a_min, double a_max,
				int b_step, double b_min, double b_max, double W, int M, int L, int K)
{
	double a_delta, b_delta;
	int i, j;
	est->a_step = a_step;
	est->b_step = b_step;
	est->a_min = a_min;
	est->b_min = b_min;
	est->a_max = a_max;
	est->b_max = b_max;
	a_delta = est->a_delta = (a_max - a_min) / a_step;
	b_delta = est->b_delta = (b_max - b_min) / b_step;
	est->a_table = type_matrix_alloc(double , a_step, b_step);
	est->b_table = type_matrix_alloc(double , a_step, b_step);

	for (i = 0; i < a_step; i++)
	{
		for (j = 0; j < b_step; j++)
		{
			localdim(a_min + i * a_delta,
				b_min + j * b_delta,
				&est->a_table[i][j],
				&est->b_table[i][j],
				W, M, L, K);
		}
	}

	return 0;
}

max_stars_repo_name: wustl-pctg/PORRidge

max_issues_repo_name: wustl-pctg/PORRidge

max_forks_repo_name: wustl-pctg/PORRidge


text:
/* The MIT License

   Copyright (c) 2013-2015 Genome Research Ltd.

   Author: Petr Danecek <pd3@sanger.ac.uk>

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in
   all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   THE SOFTWARE.

 */

#include "peakfit.h"
#include <stdio.h>
#include <gsl/gsl_version.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit_nlin.h>
#include <htslib/hts.h>
#include <htslib/kstring.h>
#include <assert.h>
#include <math.h>

#define NPARAMS 5

// gauss params: sqrt(scale), center, sigma
typedef struct _peak_t
{
    int fit_mask;
    double params[NPARAMS], ori_params[NPARAMS];    // current and input parameters
    struct { int scan; double min, max, best; } mc[NPARAMS];    // monte-carlo settings and best parameter
    void (*calc_f)  (int nvals, double *xvals, double *yvals, void *args);
    void (*calc_df) (int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args);
    void (*print_func) (struct _peak_t *pk, kstring_t *str);
    void (*convert_get) (struct _peak_t *pk, double *params);
    double (*convert_set) (struct _peak_t *pk, int iparam, double value);
}
peak_t;

struct _peakfit_t
{
    int npeaks, mpeaks, nparams, mparams;
    peak_t *peaks;
    double *params;
    int nvals, mvals;
    double *xvals, *yvals, *vals;
    kstring_t str;
    int verbose, nmc_iter;
};

/*
    Gaussian peak with the center bound in the interval <d,e>:
        yi = scale^2 * exp(-(xi-z)^2/sigma^2)

        dy/dscale  = 2*scale * EXP
        dy/dcenter = -scale^2 * sin(center) * (e-d) * (xi - z) * EXP / sigma^2
        dy/dsigma  = 2*scale^2 * (xi - z)^2 * EXP / sigma^3

    where
        z   = 0.5*(cos(center)+1)*(e-d) + d
        EXP = exp(-(xi-z)^2/sigma^2)
*/
void bounded_gaussian_calc_f(int nvals, double *xvals, double *yvals, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale2 = pk->params[0] * pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];
    double d = pk->params[3];
    double e = pk->params[4];
    double z = 0.5*(cos(center)+1)*(e-d) + d;

    int i;
    for (i=0; i<nvals; i++)
    {
        double tmp = (xvals[i] - z)/sigma;
        yvals[i] += scale2 * exp(-tmp*tmp);
    }
}
void bounded_gaussian_calc_df(int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale  = pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];
    double d = pk->params[3];
    double e = pk->params[4];
    double z = 0.5*(cos(center)+1)*(e-d) + d;

    int i;
    for (i=0; i<nvals; i++)
    {
        double EXP = exp(-(xvals[i]-z)*(xvals[i]-z)/sigma/sigma);
        double zi  = xvals[i] - z;
        if ( idf==0 )       // dscale
            dfvals[i] += 2*scale*EXP;
        else if ( idf==1 )  // dcenter
            dfvals[i] -= scale*scale*sin(center)*(e-d)*zi*EXP/sigma/sigma;
        else if ( idf==2 )  // dsigma
            dfvals[i] += 2*scale*scale*zi*zi*EXP/sigma/sigma/sigma;
    }
}
void bounded_gaussian_sprint_func(peak_t *pk, kstring_t *str)
{
    double center = pk->params[1];
    double d = pk->params[3];
    double e = pk->params[4];
    double z = 0.5*(cos(center)+1)*(e-d) + d;
    ksprintf(str,"%f**2 * exp(-(x-%f)**2/%f**2)",fabs(pk->params[0]),z,fabs(pk->params[2]));
}
double bounded_gaussian_convert_set(peak_t *pk, int iparam, double value)
{
    if ( iparam!=1 ) return value;
    double d = pk->ori_params[3];
    double e = pk->ori_params[4];
    if ( value<d ) value = d;
    else if ( value>e ) value = e;
    return acos(2*(value-d)/(e-d) - 1);
}
void bounded_gaussian_convert_get(peak_t *pk, double *params)
{
    params[0] = fabs(pk->params[0]);
    params[2] = fabs(pk->params[2]);
    double center = pk->params[1];
    double d = pk->params[3];
    double e = pk->params[4];
    params[1] = 0.5*(cos(center)+1)*(e-d) + d;
}

void peakfit_add_bounded_gaussian(peakfit_t *pkf, double a, double b, double c, double d, double e, int fit_mask)
{
    pkf->npeaks++;
    hts_expand0(peak_t,pkf->npeaks,pkf->mpeaks,pkf->peaks);

    int i, nfit = 0;
    for (i=0; i<NPARAMS; i++)
        if ( fit_mask & (1<<i) ) nfit++;

    assert( d<e );

    pkf->nparams += nfit;
    hts_expand0(double,pkf->nparams,pkf->mparams,pkf->params);

    peak_t *pk = &pkf->peaks[pkf->npeaks-1];
    memset(pk, 0, sizeof(peak_t));

    pk->calc_f      = bounded_gaussian_calc_f;
    pk->calc_df     = bounded_gaussian_calc_df;
    pk->print_func  = bounded_gaussian_sprint_func;
    pk->convert_set = bounded_gaussian_convert_set;
    pk->convert_get = bounded_gaussian_convert_get;
    pk->fit_mask    = fit_mask;
    pk->ori_params[0] = a;
    pk->ori_params[2] = c;
    pk->ori_params[3] = d;
    pk->ori_params[4] = e;
    pk->ori_params[1] = pk->convert_set(pk, 1, b);
}

/*
    Gaussian peak:
        yi = scale^2 * exp(-(x-center)^2/sigma^2)

        dy/dscale  = 2 * scale * EXP
        dy/dcenter = 2 * scale^2 * (x-center) * EXP / sigma^2
        dy/dsigma  = 2 * scale^2 * (x-center)^2 * EXP / sigma^3

    where
        EXP = exp(-(x-center)^2/sigma^2)
*/
void gaussian_calc_f(int nvals, double *xvals, double *yvals, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale2 = pk->params[0] * pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        double tmp = (xvals[i] - center)/sigma;
        yvals[i] += scale2 * exp(-tmp*tmp);
    }
}
void gaussian_calc_df(int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale  = pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        double zi  = xvals[i] - center;
        double EXP = exp(-zi*zi/(sigma*sigma));
        if ( idf==0 )       // dscale
            dfvals[i] += 2*scale*EXP;
        else if ( idf==1 )  // dcenter
            dfvals[i] += 2*scale*scale*zi*EXP/(sigma*sigma);
        else if ( idf==2 )  // dsigma
            dfvals[i] += 2*scale*scale*zi*zi*EXP/(sigma*sigma*sigma);
    }
}
void gaussian_sprint_func(struct _peak_t *pk, kstring_t *str)
{
    ksprintf(str,"%f**2 * exp(-(x-%f)**2/%f**2)",fabs(pk->params[0]),pk->params[1],fabs(pk->params[2]));
}
void gaussian_convert_get(peak_t *pk, double *params)
{
    params[0] = fabs(pk->params[0]);
    params[1] = fabs(pk->params[1]);
    params[2] = fabs(pk->params[2]);
}

void peakfit_add_gaussian(peakfit_t *pkf, double a, double b, double c, int fit_mask)
{
    pkf->npeaks++;
    hts_expand0(peak_t,pkf->npeaks,pkf->mpeaks,pkf->peaks);

    int i, nfit = 0;
    for (i=0; i<NPARAMS; i++)
        if ( fit_mask & (1<<i) ) nfit++;

    pkf->nparams += nfit;
    hts_expand0(double,pkf->nparams,pkf->mparams,pkf->params);

    peak_t *pk = &pkf->peaks[pkf->npeaks-1];
    memset(pk, 0, sizeof(peak_t));

    pk->calc_f      = gaussian_calc_f;
    pk->calc_df     = gaussian_calc_df;
    pk->print_func  = gaussian_sprint_func;
    pk->convert_get = gaussian_convert_get;
    pk->fit_mask    = fit_mask;
    pk->ori_params[0] = a;
    pk->ori_params[1] = b;
    pk->ori_params[2] = c;
}

/*
    exp peak:
        yi = scale^2 * exp((x-center)/sigma^2)

        dy/dscale  = 2 * scale * EXP
        dy/dcenter = -scale^2  * EXP / sigma^2
        dy/dsigma  = -2 * scale^2 * (x-center) * EXP / sigma^3

    where
        EXP = exp((x-center)/sigma^2)
*/
void exp_calc_f(int nvals, double *xvals, double *yvals, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale2 = pk->params[0] * pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        yvals[i] += scale2 * exp((xvals[i]-center)/sigma/sigma);
    }
}
void exp_calc_df(int nvals, double *xvals, double *yvals, double *dfvals, int idf, void *args)
{
    peak_t *pk = (peak_t*) args;

    double scale  = pk->params[0];
    double center = pk->params[1];
    double sigma  = pk->params[2];

    int i;
    for (i=0; i<nvals; i++)
    {
        double EXP = exp((xvals[i]-center)/sigma/sigma);
        if ( idf==0 )       // dscale
            dfvals[i] += 2*scale*EXP;
        else if ( idf==2 )  // dsigma
            dfvals[i] -= 2*scale*scale*(xvals[i]-center)*EXP/sigma/sigma/sigma;
    }
}
void exp_sprint_func(struct _peak_t *pk, kstring_t *str)
{
    ksprintf(str,"%f**2 * exp((x-%f)/%f**2)",fabs(pk->params[0]),pk->params[1],fabs(pk->params[2]));
}
void exp_convert_get(peak_t *pk, double *params)
{
    params[0] = fabs(pk->params[0]);
    params[1] = fabs(pk->params[1]);
    params[2] = fabs(pk->params[2]);
}

void peakfit_add_exp(peakfit_t *pkf, double a, double b, double c, int fit_mask)
{
    pkf->npeaks++;
    hts_expand0(peak_t,pkf->npeaks,pkf->mpeaks,pkf->peaks);

    int i, nfit = 0;
    for (i=0; i<NPARAMS; i++)
        if ( fit_mask & (1<<i) ) nfit++;

    assert( !(fit_mask&2) );

    pkf->nparams += nfit;
    hts_expand0(double,pkf->nparams,pkf->mparams,pkf->params);

    peak_t *pk = &pkf->peaks[pkf->npeaks-1];
    memset(pk, 0, sizeof(peak_t));

    pk->calc_f      = exp_calc_f;
    pk->calc_df     = exp_calc_df;
    pk->print_func  = exp_sprint_func;
    pk->convert_get = exp_convert_get;
    pk->fit_mask    = fit_mask;
    pk->ori_params[0] = a;
    pk->ori_params[1] = b;
    pk->ori_params[2] = c;
}

void peakfit_set_params(peakfit_t *pkf, int ipk, double *params, int nparams)
{
    peak_t *pk = &pkf->peaks[ipk];
    int i;
    if ( pk->convert_set )
        for (i=0; i<nparams; i++) pk->params[i] = pk->convert_set(pk, i, params[i]);
    else
        for (i=0; i<nparams; i++) pk->params[i] = params[i];
}

void peakfit_get_params(peakfit_t *pkf, int ipk, double *params, int nparams)
{
    peak_t *pk = &pkf->peaks[ipk];
    if ( pk->convert_get ) pk->convert_get(pk, params);
    else
    {
        int i;
        for (i=0; i<nparams; i++) params[i] = pk->params[i];
    }
}

peakfit_t *peakfit_init(void)
{
    return (peakfit_t*)calloc(1,sizeof(peakfit_t));
}

void peakfit_reset(peakfit_t *pkf)
{
    pkf->npeaks = pkf->nparams = 0;
    memset(pkf->peaks,0,sizeof(peak_t)*pkf->mpeaks);
}

void peakfit_destroy(peakfit_t *pkf)
{
    free(pkf->str.s);
    free(pkf->vals);
    free(pkf->params);
    free(pkf->peaks);
    free(pkf);
}

int peakfit_calc_f(const gsl_vector *params, void *data, gsl_vector *yvals)
{
    peakfit_t *pkf = (peakfit_t *) data;

    int i,j;
    for (i=0; i<pkf->nvals; i++)
        pkf->vals[i] = 0;

    int iparam = 0;
    for (i=0; i<pkf->npeaks; i++)
    {
        peak_t *pk = &pkf->peaks[i];
        for (j=0; j<NPARAMS; j++)
        {
            if ( !(pk->fit_mask & (1<<j)) ) continue;
            pk->params[j] = gsl_vector_get(params,iparam);
            iparam++;
        }
        pk->calc_f(pkf->nvals, pkf->xvals, pkf->vals, pk);
    }

    for (i=0; i<pkf->nvals; i++)
        gsl_vector_set(yvals, i, (pkf->vals[i] - pkf->yvals[i])/0.01);

    return GSL_SUCCESS;
}
int peakfit_calc_df(const gsl_vector *params, void *data, gsl_matrix *jacobian)
{
    peakfit_t *pkf = (peakfit_t *) data;

    int i,j,k,iparam = 0;
    for (i=0; i<pkf->npeaks; i++)
    {
        peak_t *pk = &pkf->peaks[i];
        int iparam_prev = iparam;
        for (j=0; j<NPARAMS; j++)
        {
            if ( !(pk->fit_mask & (1<<j)) ) continue;
            pk->params[j] = gsl_vector_get(params,iparam);
            iparam++;
        }
        iparam = iparam_prev;
        for (j=0; j<NPARAMS; j++)
        {
            if ( !(pk->fit_mask & (1<<j)) ) continue;
            for (k=0; k<pkf->nvals; k++) pkf->vals[k] = 0;
            pk->calc_df(pkf->nvals, pkf->xvals, pkf->yvals, pkf->vals, j, pk);
            for (k=0; k<pkf->nvals; k++) gsl_matrix_set(jacobian, k, iparam, pkf->vals[k]);
            iparam++;
        }
    }
    return GSL_SUCCESS;
}
int peakfit_calc_fdf(const gsl_vector *params, void *data, gsl_vector *yvals, gsl_matrix *jacobian)
{
    peakfit_calc_f(params, data, yvals);
    peakfit_calc_df(params, data, jacobian);
    return GSL_SUCCESS;
}

double peakfit_evaluate(peakfit_t *pkf)
{
    int i;
    for (i=0; i<pkf->nvals; i++)
        pkf->vals[i] = 0;

    for (i=0; i<pkf->npeaks; i++)
        pkf->peaks[i].calc_f(pkf->nvals, pkf->xvals, pkf->vals, &pkf->peaks[i]);

    double sum = 0;
    for (i=0; i<pkf->nvals; i++)
        sum += fabs(pkf->vals[i] - pkf->yvals[i]);

    return sum;
}

const char *peakfit_sprint_func(peakfit_t *pkf)
{
    pkf->str.l = 0;
    int i;
    for (i=0; i<pkf->npeaks; i++)
    {
        if ( i>0 ) kputs(" + ", &pkf->str);
        pkf->peaks[i].print_func(&pkf->peaks[i], &pkf->str);
    }
    return (const char*)pkf->str.s;
}

void peakfit_verbose(peakfit_t *pkf, int level)
{
    pkf->verbose = level;
}

void peakfit_set_mc(peakfit_t *pkf, double xmin, double xmax, int iparam, int niter)
{
    peak_t *pk = &pkf->peaks[ pkf->npeaks-1 ];
    pk->mc[iparam].scan = 1;
    pk->mc[iparam].min  = xmin;
    pk->mc[iparam].max  = xmax;
    pkf->nmc_iter = niter;
}

double peakfit_run(peakfit_t *pkf, int nvals, double *xvals, double *yvals)
{
    srand(0);   // for reproducibility

    pkf->nvals = nvals;
    pkf->xvals = xvals;
    pkf->yvals = yvals;
    hts_expand0(double,pkf->nvals,pkf->mvals,pkf->vals);
    if ( !pkf->nparams ) return peakfit_evaluate(pkf);

    gsl_multifit_function_fdf mfunc;
    mfunc.f   = &peakfit_calc_f;
    mfunc.df  = &peakfit_calc_df;
    mfunc.fdf = &peakfit_calc_fdf;
    mfunc.n   = nvals;
    mfunc.p   = pkf->nparams;
    mfunc.params = pkf;

    const gsl_multifit_fdfsolver_type *solver_type;
    gsl_multifit_fdfsolver *solver;
    solver_type = gsl_multifit_fdfsolver_lmsder;
    solver = gsl_multifit_fdfsolver_alloc(solver_type, nvals, mfunc.p);
    gsl_vector *grad = gsl_vector_alloc(pkf->nparams);

    int imc_iter, i,j, iparam;
    double best_fit = HUGE_VAL;
    for (imc_iter=0; imc_iter<=pkf->nmc_iter; imc_iter++)   // possibly multiple monte-carlo iterations
    {
        // set GSL parameters
        iparam = 0;
        for (i=0; i<pkf->npeaks; i++)
        {
            peak_t *pk = &pkf->peaks[i];
            for (j=0; j<NPARAMS; j++)
            {
                pk->params[j] = pk->ori_params[j];
                if ( pk->mc[j].scan )
                {
                    pk->params[j] = rand()*(pk->mc[j].max - pk->mc[j].min)/RAND_MAX + pk->mc[j].min;
                    if ( pk->convert_set ) pk->params[j] = pk->convert_set(pk, j, pk->params[j]);
                }
                if ( !(pk->fit_mask & (1<<j)) ) continue;
                pkf->params[iparam] = pk->params[j];
                iparam++;
            }
        }

        gsl_vector_view vview = gsl_vector_view_array(pkf->params, mfunc.p);
        gsl_multifit_fdfsolver_set(solver, &mfunc, &vview.vector);

        // iterate until convergence (or lack of it)
        int ret, test1 = 0, test2 = 0, niter = 0, niter_max = 500;
        do
        {
            ret = gsl_multifit_fdfsolver_iterate(solver);
            if ( pkf->verbose >1 )
            {
                fprintf(stderr, "%d: ", niter);
                for (i=0; i<pkf->npeaks; i++)
                {
                    peak_t *pk = &pkf->peaks[i];
                    fprintf(stderr,"\t%f %f %f", pk->params[0],pk->params[1],pk->params[2]);
                }
                fprintf(stderr, "\t.. %s\n", gsl_strerror(ret));
            }
            if ( ret ) break;

#if GSL_MAJOR_VERSION >= 2
            int info;
            test1 = gsl_multifit_fdfsolver_test(solver, 1e-8,1e-8, 0.0, &info);
#else
            gsl_multifit_gradient(solver->J, solver->f, grad);
            test1 = gsl_multifit_test_gradient(grad, 1e-8);
            test2 = gsl_multifit_test_delta(solver->dx, solver->x, 1e-8, 1e-8);
#endif
        }
        while ((test1==GSL_CONTINUE || test2==GSL_CONTINUE) && ++niter<niter_max);
        if ( pkf->verbose >1 )
        {
            fprintf(stderr,"test1=%s\n", gsl_strerror(test1));
            fprintf(stderr,"test2=%s\n", gsl_strerror(test2));
        }

        // recover parameters
        iparam = 0;
        for (i=0; i<pkf->npeaks; i++)
        {
            peak_t *pk = &pkf->peaks[i];
            for (j=0; j<NPARAMS; j++)
            {
                if ( !(pk->fit_mask & (1<<j)) ) continue;
                pk->params[j] = gsl_vector_get(solver->x, iparam++);
            }
        }

        // evaluate fit, update best parameters
        double fit = peakfit_evaluate(pkf);
        if ( fit<best_fit )
        {
            for (i=0; i<pkf->npeaks; i++)
            {
                peak_t *pk = &pkf->peaks[i];
                for (j=0; j<NPARAMS; j++) pk->mc[j].best = pk->params[j];
            }
        }
        if ( fit<best_fit ) best_fit = fit;
    }
    gsl_multifit_fdfsolver_free(solver);
    gsl_vector_free(grad);

    for (i=0; i<pkf->npeaks; i++)
    {
        peak_t *pk = &pkf->peaks[i];
        for (j=0; j<NPARAMS; j++) pk->params[j] = pk->mc[j].best;
    }
    return best_fit;
}

max_stars_repo_name: benranco/test

max_issues_repo_name: benranco/test

max_forks_repo_name: benranco/test


text:
/* Copyright (c) 2011-2012, Jérémy Fix. All rights reserved. */

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

/* * Redistributions of source code must retain the above copyright notice, */
/* this list of conditions and the following disclaimer. */
/* * Redistributions in binary form must reproduce the above copyright notice, */
/* this list of conditions and the following disclaimer in the documentation */
/* and/or other materials provided with the distribution. */
/* * None of the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. */

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

#ifndef UKF_TYPES_H
#define UKF_TYPES_H

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

namespace ukf
{

    /**
      * @short The different types of implemented process noise for UKF state estimation
      */
    typedef enum
    {
        /**
          * @brief The covariance of the evolution noise is fixed to \f$\textbf{P}_{\theta\theta_i} = \alpha . \textbf{I}\f$
          */
        UKF_PROCESS_FIXED,
        /**
          * @brief The covariance of the evolution noise is defined as \f$\textbf{P}_{\theta\theta_i} = (\alpha^{-1} - 1)\textbf{P}_i\f$
          */
        UKF_PROCESS_RLS
    } ProcessNoise;

    namespace parameter
    {
        class EvolutionNoise;
        /**
          * @short Structure holding the parameters of the Unscented Kalman Filter
          *
          */
        typedef struct
        {
            /**
              * @short \f$\kappa \geq 0\f$, \f$\kappa = 0\f$ is a good choice. According to van der Merwe, its value is not critical
              */
            double kpa;

            /**
              * @short \f$0 \leq \alpha \leq 1\f$ : "Size" of sigma-point distribution. Should be small if the function is strongly non-linear
              */
            double alpha;

            /**
              * @short Non negative weights used to introduce knowledge about the higher order moments of the distribution. For gaussian distributions, \f$\beta = 2\f$ is a good choice
              */
            double beta;

            /**
              * @short \f$\lambda = \alpha^2 (n + \kappa) - n\f$
              */
            double lambda;

            /**
              * @short \f$\gamma = \sqrt{\lambda + n}\f$
              */
            double gamma;

            /**
              * @short Parameter used for the evolution noise
              */
            //double evolution_noise_parameter;

            /**
              * @short Initial value of the evolution noise
              */
            //double evolution_noise;

            /**
              * @short Evolution noise type
              */
            //EvolutionNoise evolution_noise_type;
            EvolutionNoise * evolution_noise;

            /**
              * @short Covariance of the observation noise
              */
            double observation_noise;

            /**
              * @short Prior estimate of the covariance matrix
              */
            double prior_pi;

            /**
              * @short Number of parameters to estimate
              */
            int n;

            /**
              * @short \f$nbSamples = (2 n + 1)\f$ Number of sigma-points
              */
            int nbSamples;

            /**
              * @short Dimension of the output
              */
            int no;

        } ukf_param;

        /**
          * @short Pointer to the function to approximate in the scalar case
          *
          */
        //typedef double (*ukf_function_scalar) (gsl_vector * param, gsl_vector * input);

        /**
          * @short Structure holding the matrices manipulated by the statistical linearization
          * in the scalar case
          *
          */
        typedef struct
        {
            /**
              * @short Kalman gain, a vector of size \f$n\f$
              */
            gsl_vector * Kk;

            /**
              * @short Temporary matrix for the kalman gain, a matrix of size \f$(n,1)\f$
              */
            gsl_matrix * Kk_mat;

            /**
              * @short Temporary matrix for the transpose of the kalman gain, a matrix of size \f$(1,n)\f$
              */
            gsl_matrix * Kk_mat_T;

            /**
             * @short Covariance of \f$(w, d)\f$: \f$P_{w_k d_k}\f$, of size \f$n\f$
             */
            gsl_vector * Pwdk;

            /**
              * @short Covariance of the evolution noise
              */
            gsl_matrix * Prrk;

            /**
              * @short Covariance of the observation noise
              */
            double Peek;

            /**
              * @short Covariance of the output
              */
            double Pddk;

            /**
              * @short Parameter vector, of size \f$n\f$
              */
            gsl_vector * w;

            /**
              * @short Temporary vector, holding a sigma-point
              */
            gsl_vector * wk;

            /**
              * @short Covariance matrix of the parameters, of size \f$(n,n)\f$
              */
            gsl_matrix * Pk;

            /**
              * @short Matrix holding the Cholesky decomposition of \f$P_k\f$, of size \f$(n,n)\f$
              */
            gsl_matrix * Sk;

            /**
              * @short Vector holding one column of Sk, of size \f$n\f$
              */
            gsl_vector * cSk;

            /**
              * @short Weights used to compute the mean of the sigma points' images
              * @brief \f$wm_0 = \frac{\lambda}{n + \lambda}\f$
              *        \f$wm_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wm;

            /**
              * @short Weights used to update the covariance matrices
              * @brief \f$wc_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\f$
              *        \f$wc_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wc;

            /**
              * @short Temporary vector holding the image of the sigma points, of size \f$nbSamples\f$
              */
            gsl_vector * dk;

            /**
              * @short Innovation
              */
            double ino_dk;

            /**
              * @short Variable holding the mean of the sigma points image
              */
            double d_mean;

            /**
              * @short Matrix holding the sigma points in the columns, of size \f$(n,nbSamples)\f$
              */
            gsl_matrix * sigmaPoints;

            /**
              * @short Temporary vector
              */
            gsl_vector * temp_n;

            /**
              * @short Temporary matrix
              */
            gsl_matrix * temp_n_n;

        } ukf_scalar_state;

        /**
          * @short Structure holding the matrices manipulated by the unscented kalman filter
          *  in the vectorial case, for Parameter estimation
          *
          */
        typedef struct
        {
            /**
              * @short Kalman gain, a matrix of size \f$n \times no\f$
              */
            gsl_matrix * Kk;

            /**
              * @short The tranposed Kalman gain, a vector of size \f$no \times n\f$
              */
            gsl_matrix * Kk_T;

            /**
             * @short Covariance of \f$(w, d)\f$: \f$P_{w_k d_k}\f$, of size \f$n \times no\f$
             */
            gsl_matrix * Pwdk;

            /**
              * @short Covariance of the output, a matrix of size \f$ no \times no \f$
              */
            gsl_matrix * Pddk;

            /**
              * @short Covariance of the observation noise, of size \f$ no \times no \f$
              */
            gsl_matrix * Peek;

            /**
              * @short Covariance of the evolution noise, of size \f$ n \times n \f$
              */
            gsl_matrix * Prrk;

            /**
              * @short Parameter vector, of size \f$n\f$
              */
            gsl_vector * w;

            /**
              * @short Temporary vector holding one sigma point, of size \f$n\f$
              */
            gsl_vector * wk;

            /**
              * @short Covariance matrix of the parameters, of size \f$(n,n)\f$
              */
            gsl_matrix * Pk;

            /**
              * @short Matrix holding the Cholesky decomposition of \f$P_k\f$, of size \f$(n,n)\f$
              */
            gsl_matrix * Sk;

            /**
              * @short Vector holding one column of Sk, of size \f$n\f$
              */
            gsl_vector * cSk;

            /**
              * @short Weights used to compute the mean of the sigma points' images
              * @brief \f$wm_0 = \frac{\lambda}{n + \lambda}\f$
              *        \f$wm_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wm;

            /**
              * @short Weights used to update the covariance matrices
              * @brief \f$wc_0 = \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta)\f$
              *        \f$wc_i = \frac{1}{2(n + \lambda)}\f$
              */
            gsl_vector * wc;

            /**
              * @short Temporary matrix holding the image of the sigma points, of size \f$ no \times nbSamples\f$
              */
            gsl_matrix * dk;

            /**
              * @short Vector holding the mean of the sigma points image, of size \f$ no \f$ŝ
              */
            gsl_vector * d_mean;

            /**
              * @short Vector holding the inovation, of size \f$ no \f$ŝ
              */
            gsl_vector * ino_dk;

            /**
              * @short Matrix holding the sigma points in the columns, of size \f$(n,nbSamples)\f$
              */
            gsl_matrix * sigmaPoints;

            /**
              * @short Temporary vector of size \f$ n \f$
              */
            gsl_vector * vec_temp_n;

            /**
              * @short Temporary vector of size \f$ no \f$
              */
            gsl_vector * vec_temp_output;

            /**
              * @short Temporary matrix of size \f$ n \times 1 \f$
              */
            gsl_matrix * mat_temp_n_1;

            /**
              * @short Temporary matrix of size \f$ n \times no \f$
              */
            gsl_matrix * mat_temp_n_output;

            /**
              * @short Temporary matrix of size \f$ no \times n \f$
              */
            gsl_matrix * mat_temp_output_n;

            /**
              * @short Temporary matrix of size \f$ 1 \times no \f$
              */
            gsl_matrix * mat_temp_1_output;

            /**
              * @short Temporary matrix of size \f$ no \times 1 \f$
              */
            gsl_matrix * mat_temp_output_1;

            /**
              * @short Temporary matrix of size \f$ no \times no \f$
              */
            gsl_matrix * mat_temp_output_output;

            /**
              * @short Temporary matrix of size \f$ n \times n \f$
              */
            gsl_matrix * mat_temp_n_n;
        } ukf_state;

        /**
          * @short Mother class from which the evolution noises inherit
          */
        class EvolutionNoise
        {
        protected:
            double _initial_value;
        public:
            EvolutionNoise(double initial_value) : _initial_value(initial_value) {};
            void init(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_set_identity(s.Prrk);
                gsl_matrix_scale(s.Prrk, _initial_value);
            }
            void init(ukf_param &p, ukf_scalar_state &s)
            {
                gsl_matrix_set_identity(s.Prrk);
                gsl_matrix_scale(s.Prrk, _initial_value);
            }

            virtual void updateEvolutionNoise(ukf_param &p, ukf_state &s) = 0;
            virtual void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s) = 0;
        };

        /**
          * @short Annealing type evolution noise
          */
        class EvolutionAnneal : public EvolutionNoise
        {
            double _decay, _lower_bound;
        public:
            EvolutionAnneal(double initial_value, double decay, double lower_bound) : EvolutionNoise(initial_value),
            _decay(decay),
            _lower_bound(lower_bound)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                            gsl_matrix_set(s.Prrk, i, i, ukf::math::max(_decay * gsl_matrix_get(s.Prrk,i,i),_lower_bound));
                        else
                            gsl_matrix_set(s.Prrk, i, j, 0.0);
                    }
                }
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                            gsl_matrix_set(s.Prrk, i, i, ukf::math::max(_decay * gsl_matrix_get(s.Prrk,i,i),_lower_bound));
                        else
                            gsl_matrix_set(s.Prrk, i, j, 0.0);
                    }
                }
            }
        };

        /**
          * @short Forgetting type evolution noise
          */
        class EvolutionRLS : public EvolutionNoise
        {
            double _decay;
        public:
            EvolutionRLS(double initial_value, double decay) : EvolutionNoise(initial_value), _decay(decay)
            {
                if(ukf::math::cmp_equal(_decay, 0.0))
                    printf("Forgetting factor should not be null !!\n");
            };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_memcpy(s.Prrk, s.Pk);
                gsl_matrix_scale(s.Prrk, 1.0 / _decay - 1.0);
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                gsl_matrix_memcpy(s.Prrk, s.Pk);
                gsl_matrix_scale(s.Prrk, 1.0 / _decay - 1.0);
            }
        };

        /**
          * @short Robbins-Monro evolution noise
          */
        class EvolutionRobbinsMonro : public EvolutionNoise
        {
            double _alpha;
        public:
            EvolutionRobbinsMonro(double initial_value, double alpha) : EvolutionNoise(initial_value),
            _alpha(alpha)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                // Compute Kk * ino_yk
	      gsl_matrix_view mat_view = gsl_matrix_view_array(s.ino_dk->data, p.no, 1);
	      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Kk, &mat_view.matrix, 0.0, s.mat_temp_n_1);
	      // Compute : Prrk = (1 - alpha) Prrk + alpha . (Kk * ino_dk) * (Kk * ino_dk)^T
	      gsl_blas_dgemm(CblasNoTrans, CblasTrans, _alpha, s.mat_temp_n_1, s.mat_temp_n_1, (1.0 - _alpha),s.Prrk);
            }
            void updateEvolutionNoise(ukf_param &p, ukf_scalar_state &s)
            {
                // Compute  Prrk = (1 - alpha) Prrk + alpha . ino_dk^2 Kk Kk^T
	      gsl_matrix_view mat_view = gsl_matrix_view_array(s.Kk->data,p.n,1);
                gsl_blas_dgemm(CblasNoTrans, CblasTrans, gsl_pow_2(s.ino_dk) * _alpha, &mat_view.matrix, &mat_view.matrix, 1.0 - _alpha, s.Prrk);
            }
        };

    } // parameter

    namespace state
    {

        class EvolutionNoise;
        /**
          * @short Structure holding the parameters of the statistical linearization
          *
          */
        typedef struct
        {
            /**
              * @short \f$\kappa \geq 0\f$, \f$\kappa = 0\f$ is a good choice. According to van der Merwe, its value is not critical
              */
            double kpa;

            /**
              * @short \f$0 \leq \alpha \leq 1\f$ : "Size" of sigma-point distribution. Should be small if the function is strongly non-linear
              */
            double alpha;

            /**
              * @short Non negative weights used to introduce knowledge about the higher order moments of the distribution. For gaussian distributions, \f$\beta = 2\f$ is a good choice
              */
            double beta;

            /**
              * @short \f$\lambda = \alpha^2 (n + \kappa) - n\f$
              */
            double lambda;
            double lambda_aug;

            /**
              * @short \f$\gamma = \sqrt{\lambda + n}\f$
              */
            double gamma;
            double gamma_aug;

            /**
              * @short Size of the state vector
              */
            int n;

            /**
              * @short \f$nbSamples = (2 n + 1)\f$ Number of sigma-points
              */
            int nbSamples;

            /**
              * @short \f$nbSamplesMeasure = (4 n + 1)\f$ Number of sigma-points
              */
            int nbSamplesMeasure;

            /**
              * @short Dimension of the output : the measurements
              */
            int no;

            /**
              * @short Prior estimate of the covariance matrix of the state
              */
            double prior_x;

            /**
              * @short Type of process noise
              */
            EvolutionNoise * evolution_noise;

            /**
              * @short Parameter used for the evolution noise
              */
            //double process_noise;

            /**
              * @short Covariance of the observation noise
              */
            double measurement_noise;

        } ukf_param;

        /**
          * @short Structure holding the matrices manipulated by the statistical linearization
          *  in the vectorial case for state estimation
          *
          */
        typedef struct
        {
            gsl_vector * params; // The parameters of the process equation or anything you need to give to the process function

            gsl_vector * xi; // State vector
            gsl_matrix * xi_prediction; // Prediction of the states for each sigma-point
            gsl_vector * xi_mean; // Mean state vector
            gsl_matrix * Pxxi; // State covariance matrix
            gsl_matrix * cholPxxi; // Cholesky decomposition of Pxxi
            gsl_matrix * Pvvi; // Process noise covariance
            gsl_matrix * cholPvvi; // Cholesky decomposition of the process noise

            gsl_matrix * yi_prediction; // Prediction of the measurements for each sigma-point
            gsl_vector * yi_mean; // Mean measurement vector
            gsl_vector * ino_yi; // Innovations
            gsl_matrix * Pyyi; // Measurement covariance matrix
            gsl_matrix * Pnni; // Measurement noise covariance

            gsl_matrix * Pxyi; // State-Measurement covariance matrix

            gsl_vector * sigmaPoint; // Vector holding one sigma point
            gsl_matrix * sigmaPoints;  // Matrix holding all the sigma points

            gsl_vector * sigmaPointMeasure; // one sigma point for the observation equation
            gsl_matrix * sigmaPointsMeasure; // Sigma points for the observation equation

            gsl_vector * wm_j; // Weights used to compute the mean of the sigma points images
            gsl_vector * wc_j; // Weights used to update the covariance matrices

            gsl_vector * wm_aug_j; // Augmented set of Weights used to compute the mean of the sigma points images for the observations
            gsl_vector * wc_aug_j; // Augmented set of Weights used to update the covariance matrices of the observations

            gsl_matrix * Ki; // Kalman gain
            gsl_matrix * Ki_T; // Its transpose

            gsl_vector * temp_n;
            gsl_matrix * temp_n_n;
            gsl_matrix * temp_n_1;
            gsl_matrix * temp_1_n;

            gsl_matrix * temp_n_no;

            gsl_matrix * temp_no_no;
            gsl_matrix * temp_no_1;
            gsl_matrix * temp_1_no;
        } ukf_state;

        /**
          * @short Mother class from which the evolution noises inherit
          */
        class EvolutionNoise
        {
        protected:
            double _initial_value;
        public:
            EvolutionNoise(double initial_value) : _initial_value(initial_value) {};
            void init(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_set_identity(s.Pvvi);
                gsl_matrix_scale(s.Pvvi, _initial_value);
                gsl_matrix_set_identity(s.cholPvvi);
                gsl_matrix_scale(s.cholPvvi, sqrt(_initial_value));
            }

            virtual void updateEvolutionNoise(ukf_param &p, ukf_state &s) = 0;
        };

        /**
          * @short Annealing type evolution noise
          */
        class EvolutionAnneal : public EvolutionNoise
        {
            double _decay, _lower_bound;
        public:
            EvolutionAnneal(double initial_value, double decay, double lower_bound) : EvolutionNoise(initial_value),
            _decay(decay),
            _lower_bound(lower_bound)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                double value;
                for(int i = 0 ; i < p.n ; ++i)
                {
                    for(int j = 0 ; j < p.n ; ++j)
                    {
                        if(i == j)
                        {
                            value = ukf::math::max(_decay * gsl_matrix_get(s.Pvvi,i,i),_lower_bound);
                            gsl_matrix_set(s.Pvvi, i, i, value);
                            gsl_matrix_set(s.cholPvvi, i ,i, sqrt(value));
                        }
                        else
                        {
                            gsl_matrix_set(s.Pvvi, i, j, 0.0);
                            gsl_matrix_set(s.cholPvvi, i, j, 0.0);
                        }
                    }
                }
            }
        };

        /**
          * @short Forgetting type evolution noise
          */
        class EvolutionRLS : public EvolutionNoise
        {
            double _decay;
        public:
            EvolutionRLS(double initial_value, double decay) : EvolutionNoise(initial_value), _decay(decay)
            {
                if(ukf::math::cmp_equal(_decay, 0.0))
                    printf("Forgetting factor should not be null !!\n");
            };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                gsl_matrix_memcpy(s.Pvvi, s.Pxxi);
                gsl_matrix_scale(s.Pvvi, 1.0 / _decay - 1.0);
                gsl_matrix_memcpy(s.cholPvvi, s.Pvvi);
                gsl_linalg_cholesky_decomp(s.cholPvvi);
                // Set all the elements of cholPvvi strictly above the diagonal to zero
                for(int j = 0 ; j < p.n ; j++)
                    for(int k = j+1 ; k < p.n ; k++)
                         gsl_matrix_set(s.cholPvvi,j,k,0.0);
            }
        };

        /**
          * @short Robbins-Monro evolution noise
          */
        class EvolutionRobbinsMonro : public EvolutionNoise
        {
            double _alpha;
        public:
            EvolutionRobbinsMonro(double initial_value, double alpha) : EvolutionNoise(initial_value),
            _alpha(alpha)
            { };

            void updateEvolutionNoise(ukf_param &p, ukf_state &s)
            {
                // Compute Kk * ino_yk
                gsl_blas_dgemv(CblasNoTrans, 1.0, s.Ki, s.ino_yi, 0.0, s.temp_n);
                // Compute : Prrk = (1 - alpha) Prrk + alpha . (Kk * ino_dk) * (Kk * ino_dk)^T
		gsl_matrix_view mat_view = gsl_matrix_view_array(s.temp_n->data,p.n,1);
                gsl_blas_dgemm(CblasNoTrans, CblasTrans, _alpha, &mat_view.matrix, &mat_view.matrix, (1.0 - _alpha),s.Pvvi);
            }
        };

    } // namespace for state estimation

    namespace srstate
    {

        /**
          * @short Structure holding the parameters of the statistical linearization
          *
          */
        typedef struct
        {
            /**
              * @short \f$\kappa \geq 0\f$, \f$\kappa = 0\f$ is a good choice. According to van der Merwe, its value is not critical
              */
            double kpa;

            /**
              * @short \f$0 \leq \alpha \leq 1\f$ : "Size" of sigma-point distribution. Should be small if the function is strongly non-linear
              */
            double alpha;

            /**
              * @short Non negative weights used to introduce knowledge about the higher order moments of the distribution. For gaussian distributions, \f$\beta = 2\f$ is a good choice
              */
            double beta;

            /**
              * @short \f$\lambda = \alpha^2 (n + \kappa) - n\f$
              */
            double lambda;
            double lambda_aug;

            /**
              * @short \f$\gamma = \sqrt{\lambda + n}\f$
              */
            double gamma;
            double gamma_aug;

            /**
              * @short Size of the state vector
              */
            int n;

            /**
              * @short \f$nbSamples = (2 n + 1)\f$ Number of sigma-points
              */
            int nbSamples;

            /**
              * @short \f$nbSamplesMeasure = (4 n + 1)\f$ Number of sigma-points
              */
            int nbSamplesMeasure;

            /**
              * @short Dimension of the output : the measurements
              */
            int no;

            /**
              * @short Prior estimate of the covariance matrix of the state
              */
            double prior_x;

            /**
              * @short Type of process noise
              */
            ProcessNoise process_noise_type;

            /**
              * @short Parameter used for the evolution noise
              */
            double process_noise;

            /**
              * @short Covariance of the observation noise
              */
            double measurement_noise;

        } ukf_param;

        /**
          * @short Structure holding the matrices manipulated by the statistical linearization
          *  in the vectorial case for state estimation
          *
          */
        typedef struct
        {
            gsl_vector * params; // The parameters of the process equation or anything you need to give to the process function

            gsl_vector * xi; // State vector
            gsl_matrix * xi_prediction; // Prediction of the states for each sigma-point
            gsl_vector * xi_mean; // Mean state vector
            gsl_matrix * Sxi; // Cholesky factor of variance covariance of the state
            gsl_matrix * Svi; // Cholesky factor of the process noise

            gsl_matrix * yi_prediction; // Prediction of the measurements for each sigma-point
            gsl_vector * yi_mean; // Mean measurement vector
            gsl_vector * ino_yi; // Innovations
            gsl_matrix * Syi; // Cholesky factor of the variance covariance of the output
            gsl_matrix * Sni; // Cholesky factor of the measurement noise

            gsl_matrix * Pxyi; // State-Measurement covariance matrix

            gsl_vector * sigmaPoint; // Vector holding one sigma point
            gsl_matrix * sigmaPoints;  // Matrix holding all the sigma points

            gsl_vector * sigmaPointMeasure; // one sigma point for the observation equation
            gsl_matrix * sigmaPointsMeasure; // Sigma points for the observation equation

            gsl_matrix * U;

            gsl_vector * wm_j; // Weights used to compute the mean of the sigma points images
            gsl_vector * wc_j; // Weights used to update the covariance matrices

            gsl_vector * wm_aug_j; // Augmented set of Weights used to compute the mean of the sigma points images for the observations
            gsl_vector * wc_aug_j; // Augmented set of Weights used to update the covariance matrices of the observations

            gsl_matrix * Ki; // Kalman gain
            gsl_matrix * Ki_T; // Its transpose

            gsl_vector * temp_n;
            gsl_vector * temp_no;

            gsl_matrix * temp_n_n;
            gsl_matrix * temp_n_1;
            gsl_matrix * temp_1_n;

            gsl_matrix * temp_n_no;

            gsl_matrix * temp_no_no;
            gsl_matrix * temp_no_1;
            gsl_matrix * temp_1_no;

            gsl_matrix * temp_3n_n;
            gsl_matrix * temp_2nno_no;
        } ukf_state;

    } // namespace for state estimation, square root implementation

}

#endif // UKF_TYPES_H


text:
/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef tree_5172eda7_51f9_405a_9f3b_bcaa5e4243c1_h
#define tree_5172eda7_51f9_405a_9f3b_bcaa5e4243c1_h

#include <assert.h>
#include <gslib/std.h>
#include <gslib/string.h>

__gslib_begin__

struct _tree_trait_copy {};
struct _tree_trait_detach {};

template<class _node>
struct _treenode_list
{
public:
    typedef _node mynode;
    typedef _treenode_list<_node> mylist;

protected:
    mynode*         _first;
    mynode*         _last;
    int             _size;

public:
    _treenode_list() { reset(); }
    _treenode_list(mynode* node) { setup(node, node, 1); }
    _treenode_list(mynode* first, mynode* last, int size) { setup(first, last, size); }
    _treenode_list(mynode* first, mynode* last) { setup(first, last, count_nodes(first, last)); }
    bool empty() const { return !_size; }
    int size() const { return _size; }
    mynode* front() const { return _first; }
    mynode* back() const { return _last; }
    template<class _lambda>
    void for_each(_lambda lam) { for_each(_first, _last, lam); }

public:
    void setup(mynode* first, mynode* last, int size)
    {
        _first = first;
        _last = last;
        _size = size;
    }
    void reset()
    {
        _first = _last = nullptr;
        _size = 0;
    }
    void swap(mylist& that)
    {
        gs_swap(_first, that._first);
        gs_swap(_last, that._last);
        gs_swap(_size, that._size);
    }
    void erase(mynode* node)
    {
        assert(node);
        mynode* prev = node->prev();
        mynode* next = node->next();
        join(prev, next);
        if(node == _first)
            _first = next;
        if(node == _last)
            _last = prev;
        node->_prev = node->_next = nullptr;
        _size --;
    }
    void init(mynode* node)
    {
        assert(node);
        assert(!_first && !_last && !_size);
        _first = _last = node;
        _size = 1;
    }
    void insert_before(mynode* pos, mynode* node)
    {
        assert(node);
        if(pos == nullptr)
            return init(node);
        join(pos->prev(), node);
        join(node, pos);
        if(pos == _first)
            _first = node;
        _size ++;
    }
    void insert_after(mynode* pos, mynode* node)
    {
        assert(node);
        if(pos == nullptr)
            return init(node);
        join(node, pos->next());
        join(pos, node);
        if(pos == _last)
            _last = node;
        _size ++;
    }
    void connect(mylist& that)
    {
        if(empty())
            return swap(that);
        else if(that.empty())
            return;
        assert(!empty() && !that.empty());
        join(back(), that.front());
        _size += that.size();
        _last = that.back();
        that.reset();
    }

public:
    static int count_nodes(mynode* first, mynode* last)
    {
        assert(first && last);
        int c = 0;
        mynode* n = first;
        for(;;) {
            c ++;
            if(n == last)
                return c;
            n = n->next();
        }
        return c;
    }
    static void join(mynode* node1, mynode* node2)
    {
        if(node1 != nullptr)
            node1->_next = node2;
        if(node2 != nullptr)
            node2->_prev = node1;
    }
    template<class _lambda>
    static void for_each(mynode* first, mynode* last, _lambda lam)
    {
        if(!first)
            return;
        for(mynode* node = first; node != last; ) {
            mynode* next = node->next();
            lam(node);
            node = next;
        }
        lam(last);
    }
};

template<class _wrapper>
struct _treenode_default_wrapper
{
    typedef _wrapper wrapper;
    typedef _treenode_default_wrapper<wrapper> default_wrapper;
    typedef _treenode_list<wrapper> children;
    template<class, class, class> friend class tree;
    template<class> friend struct _treenode_list;

protected:
    wrapper*        _parent;
    wrapper*        _prev;
    wrapper*        _next;
    children        _children;

public:
    default_wrapper() { _parent = _prev = _next = nullptr; }
    int childs() const { return _children.size(); }
    wrapper* parent() const { return _parent; }
    wrapper* prev() const { return _prev; }
    wrapper* next() const { return _next; }
    wrapper* child() const { return _children.empty() ? nullptr : _children.front(); }
    wrapper* last_child() const { return _children.empty() ? nullptr : _children.back(); }
    void birth_before(wrapper* pos, wrapper* node) { return _children.insert_before(pos, node); }
    void birth_after(wrapper* pos, wrapper* node) { return _children.insert_after(pos, node); }
    void erase(wrapper* node) { return _children.erase(node); }
    void acquire_children(children& chs)
    {
        chs.for_each([this](wrapper* w) { w->_parent = static_cast<wrapper*>(this); });
        _children.swap(chs);
    }
    void swap_children(wrapper& w)
    {
        children t;
        t.swap(_children);
        w.acquire_children(t);
        acquire_children(t);
    }
    void reset_children() { _children.reset(); }
    template<class _lambda>
    void for_each(_lambda lam) { _children.for_each(lam); }
};

template<class _ty>
struct _treenode_cpy_wrapper:
    public _treenode_default_wrapper<_treenode_cpy_wrapper<_ty> >
{
    typedef _ty value;
    typedef _treenode_cpy_wrapper<_ty> wrapper;
    typedef _treenode_list<wrapper> children;
    typedef _tree_trait_copy tsf_behavior;
    template<class, class, class> friend class tree;

protected:
    value           _value;

public:
    value* get_ptr() { return &_value; }
    const value* get_ptr() const { return &_value; }
    value& get_ref() { return _value; }
    const value& get_ref() const { return _value; }
    template<class _ctor>
    void born() {}
    void born() {}
    void kill() {}
    void copy(const wrapper* a) { _value = a->_value; }
    void attach(wrapper* a) { !!error!! }
};

template<class _ty>
struct _treenode_wrapper:
    public _treenode_default_wrapper<_treenode_wrapper<_ty> >
{
    typedef _ty value;
    typedef _treenode_wrapper<_ty> wrapper;
    typedef _treenode_list<wrapper> children;
    typedef _tree_trait_detach tsf_behavior;
    template<class, class, class> friend class tree;

protected:
    value*          _value;

public:
    wrapper() { _value = nullptr; }
    value* get_ptr() const { return _value; }
    value& get_ref() const { return *_value; }
    template<class _ctor>
    void born() { _value = new _ctor; }
    template<class _ctor, class _a1>
    void born(_a1& a) { _value = new _ctor(a); }
    template<class _ctor, class _a1, class _a2>
    void born(_a1& a, _a2& b) { _value = new _ctor(a, b); }
    template<class _ctor, class _a1, class _a2, class _a3>
    void born(_a1& a, _a2& b, _a3& c) { _value = new _ctor(a, b, c); }
    void born() { _value = new value; }
    void kill() { delete _value; }
    void copy(const wrapper* a) { assert(!"prevent."); }
    void attach(wrapper* a)
    {
        assert(a && a->_value);
        kill();
        _value = a->_value;
        a->_value = nullptr;
    }
};

template<class _wrapper>
struct _tree_allocator
{
    typedef _wrapper wrapper;
    static wrapper* born() { return new wrapper; }
    static void kill(wrapper* w) { delete w; }
};

template<class _val>
struct _treenode_val
{
    typedef _val value;
    typedef const _val const_value;
    template<class, class, class> friend class tree;
    union
    {
        value*          _vptr;
        const_value*    _cvptr;
    };
    value* get_wrapper() const { return _vptr; }
    operator bool() const { return _vptr != nullptr; }

protected:
    value* vparent() const { return _vptr ? _vptr->_parent : nullptr; }
    value* vchild() const { return _vptr ? _vptr->front() : nullptr; }
    value* vchild_last() const { return _vptr ? _vptr->back() : nullptr; }
    value* vsibling() const {  return _vptr ? _vptr->next() : nullptr; }
    value* vsibling_last() const { return _vptr ? _vptr->prev() : nullptr; }

protected:
    template<class _lambda, class _value>
    static void preorder_traversal(_lambda lam, _value* v)
    {
        assert(v);
        lam(v);
        for(_value* w = v->child(); w; w = w->next())
            preorder_traversal(lam, w);
    }
    template<class _lambda, class _value>
    static void postorder_traversal(_lambda lam, _value* v)
    {
        assert(v);
        for(_value* w = v->child(); w; w = w->next())
            postorder_traversal(lam, w);
        lam(v);
    }

public:
    template<class _lambda>
    void preorder_traversal(_lambda lam) { if(_vptr) preorder_traversal(lam, _vptr); }
    template<class _lambda>
    void preorder_traversal(_lambda lam) const { if(_cvptr) preorder_traversal(lam, _cvptr); }
    template<class _lambda>
    void postorder_traversal(_lambda lam) { if(_vptr) postorder_traversal(lam, _vptr); }
    template<class _lambda>
    void postorder_traversal(_lambda lam) const { if(_cvptr) postorder_traversal(lam, _cvptr); }
};

template<class _ty,
    class _wrapper = _treenode_cpy_wrapper<_ty> >
class _tree_const_iterator:
    public _treenode_val<_wrapper>
{
public:
    typedef _ty value;
    typedef _wrapper wrapper;
    typedef _tree_const_iterator<value, wrapper> iterator;
    template<class, class, class> friend class tree;

public:
    iterator(const wrapper* w = nullptr) { _cvptr = w; }
    bool is_valid() const { return _cvptr != nullptr; }
    const value* get_ptr() const { return _cvptr->get_ptr(); }
    const value* operator->() const { return _cvptr->get_ptr(); }
    const value& operator*() const { return _cvptr->get_ref(); }
    int childs() const { return _cvptr->childs(); }
    int siblings() const { return _cvptr->parent() ? _cvptr->parent()->childs() : 1; }
    bool is_root() const { return _cvptr ? !_cvptr->parent() : false; }
    bool is_leaf() const { return _cvptr ? !_cvptr->childs() : false; }
    bool operator==(const iterator& that) const { return _cvptr == that._cvptr; }
    bool operator!=(const iterator& that) const { return _cvptr != that._cvptr; }
    iterator child() const { return _cvptr ? iterator(_cvptr->child()) : iterator(); }
    iterator last_child() const { return _cvptr ? iterator(_cvptr->last_child()) : iterator(); }
    iterator parent() const { return _cvptr ? iterator(_cvptr->parent()) : iterator(); }
    iterator next() const { return _cvptr ? iterator(_cvptr->next()) : iterator(); }
    iterator prev() const { return _cvptr ? iterator(_cvptr->prev()) : iterator(); }
    iterator root() const
    {
        if(const wrapper* w = _cvptr)
        {
            for( ; w->parent(); w = w->parent());
            return iterator(w);
        }
        return iterator();
    }
    iterator eldest() const
    {
        if(const wrapper* w = _cvptr ? _cvptr->parent() : nullptr)
            return iterator(w->child());
        return iterator();
    }
    iterator youngest() const
    {
        if(const wrapper* w = _cvptr ? _cvptr->parent() : nullptr)
            return iterator(w->last_child());
        return iterator();
    }
};

template<class _ty,
    class _wrapper = _treenode_cpy_wrapper<_ty> >
class _tree_iterator:
    public _tree_const_iterator<_ty, _wrapper>
{
public:
    typedef _ty value;
    typedef _wrapper wrapper;
    typedef _tree_const_iterator<value, wrapper> superref;
    typedef _tree_const_iterator<value, wrapper> const_iterator;
    typedef _tree_iterator<value, wrapper> iterator;
    template<class, class, class> friend class tree;

public:
    iterator(wrapper* w = nullptr): superref(w) {}
    value* get_ptr() const { return _vptr->get_ptr(); }
    value* operator->() const { return _vptr->get_ptr(); }
    value& operator*() const { return _vptr->get_ref(); }
    bool operator==(const iterator& that) const { return _vptr == that._vptr; }
    bool operator!=(const iterator& that) const { return _vptr != that._vptr; }
    bool operator==(const const_iterator& that) const { return _vptr == that._vptr; }
    bool operator!=(const const_iterator& that) const { return _vptr != that._vptr; }
    operator const_iterator() { return const_iterator(_cvptr); }
    void to_root() { while(wrapper* w = _vptr->parent()) _vptr = w; }
    void to_child() { _vptr = _vptr->child(); }
    void to_last_child() { _vptr = _vptr->last_child(); }
    void to_parent() { _vptr = _vptr->parent(); }
    void to_next() { _vptr = _vptr->next(); }
    void to_prev() { _vptr = _vptr->prev(); }
    void to_eldest() { if(wrapper* w = _vptr->parent()) _vptr = w->child(); }
    void to_youngest() { if(wrapper* w = _vptr->parent()) _vptr = w->last_child(); }
    iterator root() const
    {
        iterator i(_vptr);
        i.to_root();
        return i;
    }
    iterator child() const { return _vptr ? iterator(_vptr->child()) : iterator(); }
    iterator last_child() const { return _vptr ? iterator(_vptr->last_child()) : iterator(); }
    iterator parent() const { return _vptr ? iterator(_vptr->parent()) : iterator(); }
    iterator next() const { return _vptr ? iterator(_vptr->next()) : iterator(); }
    iterator prev() const { return _vptr ? iterator(_vptr->prev()) : iterator(); }
    iterator eldest() const
    {
        iterator i(_vptr);
        i.to_eldest();
        return i;
    }
    iterator youngest() const
    {
        iterator i(_vptr);
        i.to_youngest();
        return i;
    }
};

template<class _ty,
    class _wrapper = _treenode_cpy_wrapper<_ty>,
    class _alloc = _tree_allocator<_wrapper> >
class tree:
    public _treenode_val<_wrapper>
{
public:
    typedef _ty value;
    typedef _wrapper wrapper;
    typedef _alloc alloc;
    typedef tree<value, wrapper, alloc> myref;
    typedef _tree_const_iterator<value, wrapper> const_iterator;
    typedef _tree_iterator<value, wrapper> iterator;
    friend class const_iterator;
    friend class iterator;

public:
    tree() { _vptr = nullptr; }
    ~tree() { destroy(); }
    void destroy() { erase(get_root()); }
    void clear() { destroy(); }
    void swap(myref& that) { gs_swap(_vptr, that._vptr); }
    iterator get_root() { return iterator(_vptr); }
    const_iterator get_root() const { return const_iterator(_cvptr); }
    bool is_valid() const { return _cvptr != nullptr; }
    iterator insert(iterator i) { return insert<value>(i); }
    iterator insert_after(iterator i) { return insert_after<value>(i); }
    iterator birth(iterator i) { return birth<value>(i); }
    iterator birth_tail(iterator i) { return birth_tail<value>(i); }

public:
    bool is_root(const_iterator i) const
    {
        if(!i.is_valid())
            return false;
        return _cvptr == i._cvptr;
    }
    bool is_mine(const_iterator i) const
    {
        if(!i.is_valid())
            return false;
        return is_root(i.root());
    }
    void set_root(wrapper* w)
    {
        assert(!_vptr && "use attach method.");
        _vptr = w;
    }
    void erase(iterator i)
    {
        if(!i.is_valid())
            return;
        if(is_root(i)) {
            _erase(i);
            _vptr = nullptr;
        }
        else {
            wrapper* w = i._vptr;
            wrapper* p = w->parent();
            p->erase(w);
            _erase(i);
        }
    }
    template<class _ctor>
    iterator insert(iterator i)
    {
        if(!i.is_valid())
            return _cvptr ? get_root() : _init<_ctor>();
        wrapper* w = i._vptr;
        wrapper* p = w->_parent;
        wrapper* n = alloc::born();
        n->born<_ctor>();
        p->birth_before(w, n);
        n->_parent = p;
        return iterator(n);
    }
    template<class _ctor>
    iterator insert_after(iterator i)
    {
        if(!i.is_valid())
            return _cvptr ? get_root() : _init<_ctor>();
        wrapper* w = i._vptr;
        wrapper* p = w->_parent;
        wrapper* n = alloc::born();
        n->born<_ctor>();
        p->birth_after(w, n);
        n->_parent = p;
        return iterator(n);
    }
    template<class _ctor>
    iterator birth(iterator i)
    {
        if(!i.is_valid())
            return _cvptr ? get_root() : _init<_ctor>();
        wrapper* w = i._vptr;
        wrapper* n = alloc::born();
        n->born<_ctor>();
        w->birth_before(w->child(), n);
        n->_parent = w;
        return iterator(n);
    }
    template<class _ctor>
    iterator birth_tail(iterator i)
    {
        if(!i.is_valid())
            return _cvptr ? get_root() : _init<_ctor>();
        wrapper* w = i._vptr;
        wrapper* n = alloc::born();
        n->born<_ctor>();
        w->birth_after(w->last_child(), n);
        n->_parent = w;
        return iterator(n);
    }
    myref& detach(myref& subtree, iterator i)
    {
        if(!i.is_valid() || !is_mine(i))
            return subtree;
        if(is_root(i))
            _vptr = nullptr;
        else {
            wrapper* m = i._vptr;
            wrapper* p = m->_parent;
            p->erase(m);
            m->_parent = nullptr;
        }
        subtree.~tree();
        new (&subtree) tree(i);
        return subtree;
    }
    template<class _tsftrait>
    void transfer(wrapper* src, wrapper* des)
    {
    }
    template<>
    void transfer<_tree_trait_copy>(wrapper* src, wrapper* des)
    {
        assert(src && des);
        des->copy(src);
        des->swap_children(*src);
        src->for_each([&](wrapper* w) { _erase(w); });
        src->reset_children();
    }
    template<>
    void transfer<_tree_trait_detach>(wrapper* src, wrapper* des)
    {
        assert(src && des);
        des->attach(src);
        des->swap_children(*src);
        src->for_each([&](wrapper* w) { _erase(w); });
        src->reset_children();
    }
    void attach(myref& subtree, iterator i)
    {
        assert(i.is_valid() && is_mine(i));
        wrapper* w = subtree.get_root()._vptr;
        assert(w);
        transfer<wrapper::tsf_behavior>(w, i._vptr);
        w = i._vptr;
        for(wrapper* c = w->child(); c; c = c->next())
            c->_parent = w;
    }
    template<class _ctor>
    iterator swap(iterator p, myref& that)
    {
        assert(this != &that);
        if(p == get_root()) {
            swap(that);
            return get_root();
        }
        iterator prevsib, grand;
        prevsib = p.prev();
        grand = p.parent();
        assert(prevsib || grand);
        myref t;
        detach(t, p);
        auto pos = prevsib ? insert_after<_ctor>(prevsib) : birth<_ctor>(grand);
        assert(pos);
        attach(that, pos);
        that.swap(t);
        return pos;
    }

public:
    template<class lambda>
    void for_each(lambda lam) { for_each(get_root(), lam); }
    template<class lambda>
    void for_each(iterator i, lambda lam)
    {
        if(i.is_valid()) {
            for(iterator j = i.child(); j.is_valid(); j.to_next())
                for_each(j, lam);
            lam(i.get_ptr());
        }
    }
    template<class lambda>
    void const_for_each(lambda lam) const { const_for_each(get_root(), lam); }
    template<class lambda>
    void const_for_each(const_iterator i, lambda lam) const
    {
        if(i.is_valid()) {
            for(const_iterator j = i.child(); j.is_valid(); j = j.next())
                for_each(j, lam);
            lam(i.get_ptr());
        }
    }

protected:
    tree(iterator i) { _vptr = i._vptr; }
    void _erase(iterator i) { _erase(i.get_wrapper()); }
    void _erase(wrapper* w)
    {
        assert(w);
        for(auto* c = w->child(); c;) {
            auto* n = c->next();
            _erase(c);
            c = n;
        }
        w->kill();
        alloc::kill(w);
    }
    template<class _ctor>
    iterator _init()
    {
        _vptr = alloc::born();
        _vptr->born<_ctor>();
        _vptr->_parent = nullptr;
        return iterator(_vptr);
    }

public:
    bool debug_check(iterator i)
    {
        iterator f = i.prev(), b = i.next();
        if(f.is_valid() && f.next() != i) { assert(0); return false; }
        if(b.is_valid() && b.prev() != i) { assert(0); return false; }
        for(iterator j = i.child(); j.is_valid(); j.to_next())
            if(j.parent() != i) { assert(0); return false; }
        return true;
    }
};

template<class _tree>
class _print_tree
{
public:
    typedef _tree tree;
    typedef typename tree::value value;
    typedef typename tree::iterator iterator;
    typedef typename tree::const_iterator const_iterator;

protected:
    tree&       _inst;
    string      _src;

public:
    _print_tree(tree& t):
        _inst(t)
    {
    }
    const gchar* print()
    {
        _src.clear();
        append(_inst.get_root(), 0);
        return _src.c_str();
    }

protected:
    string decorate(const_iterator i, int level)
    {
        if(!i.is_valid())
            return string();
        string r, a;
        for(int j = 0; j < level; j ++)
            r.append(_t("  "));
        i.is_leaf() ? r.append(_t("+ ")) : r.append(_t("- "));
        i.is_root() ? r.append(_t("root(")) : a.format(_t("level%d("), level);
        r.append(a);
        r.append(i->to_string());
        r.append(_t(")\n"));
        return r;
    }
    void append(const_iterator i, int level)
    {
        if(!i.is_valid())
            return;
        _src.append(decorate(i, level).c_str());
        level ++;
        for(i.to_child(); i.is_valid(); i.to_next())
            append(i, level);
    }
};

__gslib_end__

#endif

max_stars_repo_name: lymastee/gslib

max_issues_repo_name: lymastee/gslib

max_forks_repo_name: lymastee/gslib


text:
#include <gsl/gsl_math.h>
#include "gsl_cblas.h"
#include "cblas.h"

void
cblas_srotm (const int N, float *X, const int incX, float *Y, const int incY,
	     const float *P)
{
#define BASE float
#include "source_rotm.h"
#undef BASE
}