/* 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. */

#define DEBUGMODE true

#ifndef UKF_SR_STATE_NDIM_H
#define UKF_SR_STATE_NDIM_H

#include <gsl/gsl_linalg.h> // For the Cholesky decomposition
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>

#include "ukf_types.h"
#include "ukf_math.h"

void print_mat(const gsl_matrix * A)
{
    for(unsigned int i = 0 ; i < A->size1 ; ++i)
    {
        for(unsigned int j = 0 ; j < A ->size2 ; ++j)
            printf("%e ", gsl_matrix_get(A,i,j));

        printf("\n");
    }
}

void print_vec(const gsl_vector * A)
{
    for(unsigned int i = 0 ; i < A->size ; ++i)
    {
        printf("%e ", gsl_vector_get(A,i));
    }
    printf("\n");
}

namespace ukf
{

    /**
      * @short MUST NOT BE USED !!!!!!! Square root UKF for state estimation, additive noise case
      * The notations follow "Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models",p115, PhD, van Der Merwe
      */
    namespace srstate
    {
        /**
          * @short Allocation of the vectors/matrices and initialization
          *
          */
        void ukf_init(ukf_param &p, ukf_state &s)
        {
            // Parameters for the sigma points of the process equation
            p.nbSamples = 2 * p.n + 1;
            p.lambda = p.alpha * p.alpha * (p.n + p.kpa) - p.n;
            p.gamma = sqrt(p.n + p.lambda);

            // Parameters for the sigma points of the observation equation
            p.nbSamplesMeasure = 4 * p.n + 1;
            p.lambda_aug = p.alpha * p.alpha * (2*p.n + p.kpa) - 2*p.n;
            p.gamma_aug = sqrt(2*p.n + p.lambda_aug);

            // Init the matrices used to iterate
            s.xi = gsl_vector_alloc(p.n);
            gsl_vector_set_zero(s.xi);

            // Images of the sigma points
            s.xi_prediction = gsl_matrix_alloc(p.n, p.nbSamples);
            gsl_matrix_set_zero(s.xi_prediction);

            s.xi_mean = gsl_vector_alloc(p.n);
            gsl_vector_set_zero(s.xi_mean);

            // Cholesky factor of the variance/covariance matrix Pxxi
            s.Sxi = gsl_matrix_alloc(p.n,p.n);
            gsl_matrix_set_identity(s.Sxi);
            gsl_matrix_scale(s.Sxi, sqrt(p.prior_x));

            // Cholesky factor of the variance/covariance matrix of the process noise
            s.Svi = gsl_matrix_alloc(p.n,p.n);
            gsl_matrix_set_identity(s.Svi);
            gsl_matrix_scale(s.Svi, sqrt(p.process_noise));

            s.yi_prediction = gsl_matrix_alloc(p.no, p.nbSamplesMeasure);
            gsl_matrix_set_zero(s.yi_prediction);

            s.yi_mean = gsl_vector_alloc(p.no);
            gsl_vector_set_zero(s.yi_mean);

            s.ino_yi = gsl_vector_alloc(p.no);
            gsl_vector_set_zero(s.ino_yi);

            // Cholesky factor of the variance/covariance matrix Pyyi
            s.Syi = gsl_matrix_alloc(p.no, p.no);
            gsl_matrix_set_zero(s.Syi);

            s.Sni = gsl_matrix_alloc(p.no,p.no);
            gsl_matrix_set_identity(s.Sni);
            gsl_matrix_scale(s.Sni, sqrt(p.measurement_noise));

            s.Pxyi = gsl_matrix_alloc(p.n, p.no);
            gsl_matrix_set_zero(s.Pxyi);

            s.sigmaPoint = gsl_vector_alloc(p.n);
            gsl_vector_set_zero(s.sigmaPoint);

            s.sigmaPoints = gsl_matrix_alloc(p.n, p.nbSamples);
            gsl_matrix_set_zero(s.sigmaPoints);

            s.sigmaPointMeasure = gsl_vector_alloc(p.n);
            gsl_vector_set_zero(s.sigmaPoint);

            s.sigmaPointsMeasure = gsl_matrix_alloc(p.n, p.nbSamplesMeasure);
            gsl_matrix_set_zero(s.sigmaPointsMeasure);

            s.U = gsl_matrix_alloc(p.n, p.no);
            gsl_matrix_set_zero(s.U);

            // Weights used to update the statistics
            s.wm_j  = gsl_vector_alloc(p.nbSamples); // Weights used to compute the mean of the sigma points images
            s.wc_j = gsl_vector_alloc(p.nbSamples); // Weights used to update the covariance matrices

            // Set the weights
            gsl_vector_set(s.wm_j, 0, p.lambda / (p.n + p.lambda));
            gsl_vector_set(s.wc_j, 0, p.lambda / (p.n + p.lambda) + (1.0 - p.alpha*p.alpha + p.beta));
            for(int j = 1 ; j < p.nbSamples; j ++)
            {
                gsl_vector_set(s.wm_j, j, 1.0 / (2.0 * (p.n + p.lambda)));
                gsl_vector_set(s.wc_j, j, 1.0 / (2.0 * (p.n + p.lambda)));
            }

            // Set the weights
            s.wm_aug_j  = gsl_vector_alloc(p.nbSamplesMeasure); // Weights used to compute the mean of the sigma points images
            s.wc_aug_j = gsl_vector_alloc(p.nbSamplesMeasure); // Weights used to update the covariance matrices
            gsl_vector_set(s.wm_aug_j, 0, p.lambda_aug / (2*p.n + p.lambda_aug));
            gsl_vector_set(s.wc_aug_j, 0, p.lambda_aug / (2*p.n + p.lambda_aug) + (1.0 - p.alpha*p.alpha + p.beta));
            for(int j = 1 ; j < p.nbSamplesMeasure; j ++)
            {
                gsl_vector_set(s.wm_aug_j, j, 1.0 / (2.0 * (2*p.n + p.lambda_aug)));
                gsl_vector_set(s.wc_aug_j, j, 1.0 / (2.0 * (2*p.n + p.lambda_aug)));
            }

            s.Ki = gsl_matrix_alloc(p.n, p.no);
            s.Ki_T = gsl_matrix_alloc(p.no, p.n);

            // Allocate temporary matrices
            s.temp_n = gsl_vector_alloc(p.n);
            s.temp_no = gsl_vector_alloc(p.no);

            s.temp_n_1 = gsl_matrix_alloc(p.n,1);
            s.temp_1_n = gsl_matrix_alloc(1,p.n);
            s.temp_n_n = gsl_matrix_alloc(p.n, p.n);
            s.temp_n_no = gsl_matrix_alloc(p.n, p.no);
            s.temp_no_1 = gsl_matrix_alloc(p.no,1);
            s.temp_1_no = gsl_matrix_alloc(1,p.no);
            s.temp_no_no = gsl_matrix_alloc(p.no, p.no);

            s.temp_3n_n = gsl_matrix_alloc(3*p.n, p.n);
            s.temp_2nno_no = gsl_matrix_alloc(2*p.n + p.no, p.no);
        }

        /**
          * @short Free of memory allocation
          *
          */
        void ukf_free(ukf_param &p, ukf_state &s)
        {
            gsl_vector_free(s.xi);
            gsl_matrix_free(s.xi_prediction);
            gsl_vector_free(s.xi_mean);
            gsl_matrix_free(s.Sxi);
            gsl_matrix_free(s.Svi);

            gsl_matrix_free(s.yi_prediction);
            gsl_vector_free(s.yi_mean);
            gsl_vector_free(s.ino_yi);
            gsl_matrix_free(s.Syi);
            gsl_matrix_free(s.Sni);

            gsl_matrix_free(s.Pxyi);

            gsl_vector_free(s.sigmaPoint);
            gsl_matrix_free(s.sigmaPoints);

            gsl_vector_free(s.sigmaPointMeasure);
            gsl_matrix_free(s.sigmaPointsMeasure);

            gsl_matrix_free(s.U);

            gsl_vector_free(s.wm_j);
            gsl_vector_free(s.wc_j);

            gsl_vector_free(s.wm_aug_j);
            gsl_vector_free(s.wc_aug_j);

            gsl_matrix_free(s.Ki);
            gsl_matrix_free(s.Ki_T);

            gsl_vector_free(s.temp_n);
            gsl_vector_free(s.temp_no);

            gsl_matrix_free(s.temp_n_1);
            gsl_matrix_free(s.temp_1_n);
            gsl_matrix_free(s.temp_n_n);

            gsl_matrix_free(s.temp_n_no);
            gsl_matrix_free(s.temp_no_1);
            gsl_matrix_free(s.temp_1_no);
            gsl_matrix_free(s.temp_no_no);

            gsl_matrix_free(s.temp_3n_n);
            gsl_matrix_free(s.temp_2nno_no);
        }

        /**
          * @short UKF-additive (zero-mean) noise case, "Kalman Filtering and Neural Networks", p.233
          *
          */
        template <typename FunctProcess, typename FunctObservation>
                void ukf_iterate(ukf_param &p, ukf_state &s, FunctProcess f, FunctObservation h, gsl_vector* yi)
        {
            int i,j;
	    gsl_matrix_view mat_view;
	    gsl_vector_view vec_view;
            // ************************************************** //
            // ************ Compute the sigma points ************ //
            // ************************************************** //

            // 1- Compute the sigma points,
            // Equation (3.209)
            // sigmapoint_j = x_(i-1)
            // sigmapoint_j = x_(i-1) + gamma * Sxi_j for 1 <= j <= n
            // sigmapoint_j = x_(i-1) - gamma * Sxi_(j-(n+1)) for n+1 <= j <= 2n
            gsl_matrix_set_col(s.sigmaPoints, 0, s.xi);
            for(j = 1 ; j < p.n + 1 ; ++j)
                for(i = 0 ; i < p.n ; ++i)
                {
                gsl_matrix_set(s.sigmaPoints,i,j, s.xi->data[i] + p.gamma * gsl_matrix_get(s.Sxi, i, j-1));
                gsl_matrix_set(s.sigmaPoints,i,j+p.n, s.xi->data[i] - p.gamma * gsl_matrix_get(s.Sxi, i, j-1));
            }

            /**********************************/
            /***** Time update equations  *****/
            /**********************************/

            // Time update equations
            // 0 - Compute the image of the sigma points and the mean of these images
            gsl_vector_set_zero(s.xi_mean);
            for(j = 0 ; j < p.nbSamples ; ++j)
            {
                gsl_matrix_get_col(s.sigmaPoint, s.sigmaPoints, j);
                // Compute the image of the sigma points
                // Eq 3.210
		vec_view = gsl_matrix_column(s.xi_prediction,j);
                f(s.params, s.sigmaPoint, &vec_view.vector);

                // Update the mean, Eq (3.211)
                for(i = 0 ; i < p.n ; ++i)
                    s.xi_mean->data[i] += s.wm_j->data[j] * gsl_matrix_get(s.xi_prediction,i,j);
            }

            // 1- Compute the QR decomposition and keeps only the upper triangular part of R
            // Eq. 3.212
            // We first fill the matrix Sxk^-
            // with the transpose of what is in the brackets of qr { ... }
            for(j = 1 ; j < p.nbSamples; ++j)
            {
                for(i = 0 ; i < p.n ; ++i)
                {
                    gsl_matrix_set(s.temp_3n_n, j-1, i, sqrt(s.wc_j->data[1]) * (gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean->data[i]));
                }
            }
            // The last rows are filled with the cholesky factor of the process noise covariance
            for(j = 0 ; j < p.n ; ++j)
            {
	      mat_view = gsl_matrix_submatrix(s.temp_3n_n, p.nbSamples-1, 0, p.n, p.n);
                gsl_matrix_memcpy(&mat_view.matrix, s.Svi);
            }
            // We now perform the QR decomposition of s.temp_3n_n;
            gsl_linalg_QR_decomp(s.temp_3n_n, s.temp_n);
            // From this QR decomposition, we keep only the upper triangular part of R and copy it in s.Sxi
            for( j = 0 ; j < p.n ; ++j)
            {
                for(i = 0 ; i < j+1 ; ++i)
                    gsl_matrix_set(s.Sxi, i, j, gsl_matrix_get(s.temp_3n_n, i, j));

                for(i = j+1 ; i < p.n ; ++i)
                    gsl_matrix_set(s.Sxi, i, j, 0.0);
            }
            //gsl_matrix_transpose(s.Sxi);
            if(DEBUGMODE) print_mat(s.Sxi);
            if(DEBUGMODE) printf("\n");

            // 2- Perform the cholupdate, which is a rank one update
            // Eq. 3.213
            for(i = 0 ; i < p.n ; ++i)
                s.temp_n->data[i] = gsl_matrix_get(s.xi_prediction,i,0) - s.xi_mean->data[i];

            if(DEBUGMODE) printf("Before cholupdate 1 \n");
            gsl_matrix_transpose(s.Sxi);
            ukf::math::choleskyUpdate(s.Sxi, ukf::math::signof(s.wc_j->data[0]) * sqrt(fabs(s.wc_j->data[0])), s.temp_n);
            // Warning : Here, we keep Sxi as a triangular inferior matrix

            //gsl_matrix_transpose(s.Sxi);
            if(DEBUGMODE) printf("ok\n");
            if(DEBUGMODE) printf("Sxi after cholupdate : \n");
            if(DEBUGMODE) print_mat(s.Sxi);
            // ** Augment the sigma points ** //
            // Eq 3.124

            // 1 - Copy the images of the original sigma points
	    mat_view = gsl_matrix_submatrix(s.sigmaPointsMeasure,0,0, p.n, p.nbSamples);
            gsl_matrix_memcpy(&mat_view.matrix, s.xi_prediction);

            // 2 - Compute the additional sigma points
            for(j = 0 ; j < p.n ; ++j)
            {
                for(i = 0 ; i < p.n ; ++i)
                {
                    gsl_matrix_set(s.sigmaPointsMeasure, i ,j + p.nbSamples , gsl_matrix_get(s.xi_prediction, i, 0) + p.gamma_aug * gsl_matrix_get(s.Svi, i,j));
                    gsl_matrix_set(s.sigmaPointsMeasure, i ,j + p.nbSamples + p.n , gsl_matrix_get(s.xi_prediction, i, 0) - p.gamma_aug * gsl_matrix_get(s.Svi, i,j));
                }
            }

            // Compute the image of the sigma points, i.e. the associated predicted observations
            // and their mean
            gsl_vector_set_zero(s.yi_mean);
            for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
            {
                gsl_matrix_get_col(s.sigmaPointMeasure, s.sigmaPointsMeasure, j);
		vec_view = gsl_matrix_column(s.yi_prediction,j);
                h(s.sigmaPointMeasure, &vec_view.vector);

                // Update the mean , eq (3.176)
                for(i = 0 ; i < p.no ; ++i)
                    s.yi_mean->data[i] += s.wm_aug_j->data[j] * gsl_matrix_get(s.yi_prediction,i,j);
            }

            /*****************************************/
            /***** Measurement update equations  *****/
            /*****************************************/

            // Compute the Cholesky factor of the variance-covariance matrix of the observations
            // Eq. 3.217
            // First put in s.temp_2nno_no the transpose of what is in the brackets of qr{...} ,
            for(j = 0 ; j < 2*p.n ; ++j)
                for(i = 0 ; i < p.no ; ++i)
                    gsl_matrix_set(s.temp_2nno_no, j, i, sqrt(s.wc_aug_j->data[1])*(gsl_matrix_get(s.yi_prediction, i, j+1) - s.yi_mean->data[i]) );

            // Copy the Cholesky factor of the covariance of the observation noise
	    mat_view = gsl_matrix_submatrix(s.temp_2nno_no,2*p.n, 0, p.no, p.no);
            gsl_matrix_memcpy(&mat_view.matrix, s.Sni);

            // We now perform the QR decomposition of s.temp_2nno_no;
            gsl_linalg_QR_decomp(s.temp_2nno_no, s.temp_no);
            // From this QR decomposition, we keep only the upper triangular part of R and copy it in s.Syi
            for( j = 0 ; j < p.no ; ++j)
            {
                for(i = 0 ; i < j+1 ; ++i)
                    gsl_matrix_set(s.Syi, i, j, gsl_matrix_get(s.temp_2nno_no, i, j));

                for(i = j+1 ; i < p.no ; ++i)
                    gsl_matrix_set(s.Syi, i, j, 0.0);
            }
            if(DEBUGMODE) printf("Syi : \n");
            if(DEBUGMODE) print_mat(s.Syi);
            if(DEBUGMODE) printf("\n");
            //gsl_matrix_transpose(s.Syi);

            // 2- Perform the cholupdate, which is a rank one update
            // Eq. 3.218
            // Syi is triangular superior
            for(i = 0 ; i < p.no ; ++i)
                s.temp_no->data[i] = gsl_matrix_get(s.yi_prediction,i,0) - s.yi_mean->data[i];
            if(DEBUGMODE) print_vec(s.temp_no);
            if(DEBUGMODE) printf("Before cholupdate 2 \n");
            gsl_matrix_transpose(s.Syi);
            ukf::math::choleskyUpdate(s.Syi, ukf::math::signof(s.wc_aug_j->data[0])*sqrt(fabs(s.wc_aug_j->data[0])), s.temp_no);
            if(DEBUGMODE) printf("ok \n");
            // Now, Syi is the lower triangular cholesky factor
            //gsl_matrix_transpose(s.Syi);
            if(DEBUGMODE) printf("Syi after cholupdate : \n");
            if(DEBUGMODE) print_mat(s.Syi);

            // Compute the state/observation covariance
            // Eq (3.219)
            gsl_matrix_set_zero(s.Pxyi);
            for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
            {
                for(i = 0 ; i < p.n ; ++i)
                    s.temp_n_1->data[i] = gsl_matrix_get(s.sigmaPointsMeasure,i,j) - s.xi_mean->data[i];

                for(i = 0 ; i < p.no ; ++i)
                    s.temp_1_no->data[i] = gsl_matrix_get(s.yi_prediction,i,j) - s.yi_mean->data[i];

                gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, s.wc_aug_j->data[j] , s.temp_n_1, s.temp_1_no, 1.0, s.Pxyi);
                //gsl_matrix_add(s.Pxyi, s.temp_n_no);
            }

            /*gsl_matrix_set_zero(s.Pxyi);
            for(j = 0 ; j < p.nbSamples ; ++j)
            {
                for(i = 0 ; i < p.n ; ++i)
                    s.temp_n_1->data[i] = gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean->data[i];

                for(i = 0 ; i < p.no ; ++i)
                    s.temp_1_no->data[i] = gsl_matrix_get(s.yi_prediction,i,j) - s.yi_mean->data[i];

                gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, s.wc_j->data[j] , s.temp_n_1, s.temp_1_no, 1.0, s.Pxyi);
                //gsl_matrix_add(s.Pxyi, s.temp_n_no);
            }*/

            // Compute the Kalman gain,
            // Eq (3.220)

            // We want to solve : Kxy . Sy Sy^T = Pxy
            // Which can be solved using : Sy Sy^T Kxy^T = Pxy^T
            // We have a number of no systems to solve of the form S x = Pxy^T[:,i] ; S^T Kxy[:,i] = x with S being triangular
            // To solve this, we make use of gsl_blas_dtrsv
            for(int i = 0 ; i < p.n ; ++i)
            {
	      vec_view = gsl_matrix_row(s.Pxyi, i);
                gsl_vector_memcpy(s.temp_no, &vec_view.vector);
                gsl_blas_dtrsv(CblasLower,CblasNoTrans,CblasNonUnit,s.Syi,s.temp_no);
                gsl_blas_dtrsv(CblasLower,CblasTrans,CblasNonUnit,s.Syi,s.temp_no);
                // And then copy the result in the ith row of Ki
		vec_view = gsl_matrix_row(s.Ki, i);
                gsl_vector_memcpy(&vec_view.vector, s.temp_no);
            }
            if(DEBUGMODE) printf("Ki : \n");
            if(DEBUGMODE) print_mat(s.Ki);

            // Correction : correct the estimation of the state
            // Eq. 3.221
            // Compute the innovations
            for(i = 0 ; i < p.no ; ++i)
                s.ino_yi->data[i] = yi->data[i] - s.yi_mean->data[i];
            // And correct the current state estimate
            gsl_blas_dgemv(CblasNoTrans, 1.0, s.Ki, s.ino_yi, 1.0, s.xi);

            // Compute the matrix U = Kk Syk
            gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0 , s.Ki, s.Syi, 0, s.U);
            if(DEBUGMODE) printf("Matrix U : \n");
            if(DEBUGMODE) print_mat(s.U);

            // Perform the cholesky updates of Sxi
            // We perform p.no updates rank-1 update with the columns of U
            if(DEBUGMODE) printf("Before cholupdate 3 \n");
            for(i = 0 ; i < p.no ; ++i)
            {
	      vec_view = gsl_matrix_column(s.U, i);
                gsl_vector_memcpy(s.temp_n, &vec_view.vector);
                ukf::math::choleskyUpdate(s.Sxi, -1.0, s.temp_n);
                if(DEBUGMODE) printf("After cholupdate %i\n", i);
                if(DEBUGMODE) print_mat(s.Sxi);
            }

            //printf("After cholupdate %i\n", i);
            //print_mat(s.Sxi);
            //if(DEBUGMODE) printf("\n");

            //
            //            // Update of the process noise
            //            switch(p.process_noise_type)
            //            {
            //            case ukf::UKF_PROCESS_FIXED:
            //                //nothing to do
            //                break;
            //            case ukf::UKF_PROCESS_RLS:
            //                gsl_matrix_memcpy(s.Pvvi, s.Pxxi);
            //                gsl_matrix_scale(s.Pvvi, 1.0/p.process_noise-1.0);
            //                gsl_matrix_memcpy(s.cholPvvi, s.Pvvi);
            //                gsl_linalg_cholesky_decomp(s.cholPvvi);
            //                for(j = 0 ; j < p.n ; j++)
            //                    for(k = j+1 ; k < p.n ; k++)
            //                        gsl_matrix_set(s.cholPvvi,j,k,0.0);
            //                break;
            //            default:
            //                printf("Warning : Unrecognized process noise type\n");
            //            }

        }

        /**
          * @short Evaluation of the output from the sigma points
          *
          */
        template <typename FunctProcess, typename FunctObservation>
                void ukf_evaluate(ukf_param &p, ukf_state &s, FunctProcess f, FunctObservation h, gsl_vector* yi)
        {

            //          int i,j,k;
            //            // ************************************************** //
            //            // ************ Compute the sigma points ************ //
            //            // ************************************************** //
            //
            //            // 0 - Compute the Cholesky decomposition of s.Pxxi
            //            gsl_matrix_memcpy(s.cholPxxi, s.Pxxi);
            //            gsl_linalg_cholesky_decomp(s.cholPxxi);
            //            // Set all the elements of cholPvvi strictly above the diagonal to zero
            //            for(j = 0 ; j < p.n ; j++)
            //                for(k = j+1 ; k < p.n ; k++)
            //                    gsl_matrix_set(s.cholPxxi,j,k,0.0);
            //
            //            // 1- Compute the sigma points,
            //            // Equation (3.170)
            //            // sigmapoint_j = x_(i-1)
            //            // sigmapoint_j = x_(i-1) + gamma * sqrt(P_i-1)_j for 1 <= j <= n
            //            // sigmapoint_j = x_(i-1) - gamma * sqrt(P_i-1)_(j-(n+1)) for n+1 <= j <= 2n
            //            gsl_matrix_set_col(s.sigmaPoints, 0, s.xi);
            //            for(j = 1 ; j < p.n+1 ; ++j)
            //                for(i = 0 ; i < p.n ; ++i)
            //                {
            //                    gsl_matrix_set(s.sigmaPoints,i,j, s.xi->data[i] + p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));
            //                    gsl_matrix_set(s.sigmaPoints,i,j+p.n, s.xi->data[i] - p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));
            //                }
            //
            //            /**********************************/
            //            /***** Time update equations  *****/
            //            /**********************************/
            //
            //            // Time update equations
            //            // 0 - Compute the image of the sigma points and the mean of these images
            //            gsl_vector_set_zero(s.xi_mean);
            //            for(j = 0 ; j < p.nbSamples ; ++j)
            //            {
            //                gsl_matrix_get_col(s.sigmaPoint, s.sigmaPoints, j);
            //                f(s.params, s.sigmaPoint, &gsl_matrix_column(s.xi_prediction,j).vector);
            //
            //                // Update the mean, Eq (3.172)
            //                for(i = 0 ; i < p.n ; ++i)
            //                    s.xi_mean->data[i] += s.wm_j->data[j] * gsl_matrix_get(s.xi_prediction,i,j);
            //            }
            //
            //            // 1 - Compute the covariance of the images and add the process noise,
            //            // Equation (3.173)
            //            // Warning, s.Pxxi will now hold P_xk^-
            //            gsl_matrix_set_zero(s.Pxxi);
            //            for(j = 0 ; j < p.nbSamples ; ++j)
            //            {
            //                for(i = 0 ; i < p.n ; ++i)
            //                    s.temp_n_1->data[i] = gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean->data[i];
            //
            //                gsl_blas_dgemm(CblasNoTrans, CblasTrans, s.wc_j->data[j] , s.temp_n_1, s.temp_n_1, 0, s.temp_n_n);
            //                gsl_matrix_add(s.Pxxi, s.temp_n_n);
            //            }
            //            // Add the covariance of the evolution noise
            //            gsl_matrix_add(s.Pxxi, s.Pvvi);
            //
            //            // Augment sigma points
            //            // Equation 3.174
            //            // First put the images of the initial sigma points
            //            gsl_matrix_memcpy(&gsl_matrix_submatrix(s.sigmaPointsMeasure, 0, 0, p.n, p.nbSamples).matrix, s.xi_prediction);
            //            // And add the additional sigma points eq. (7.56)
            //            for(j = 0 ; j < p.n ; ++j)
            //            {
            //                for(i = 0 ; i < p.n ; ++i)
            //                {
            //                    gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples, gsl_matrix_get(s.xi_prediction,i,0)+p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));
            //                    gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples+p.n, gsl_matrix_get(s.xi_prediction,i,0)-p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));
            //                }
            //            }
            //
            //            // Compute the image of the sigma points through the observation equation
            //            // eq (3.175)
            //            gsl_vector_set_zero(yi);
            //            for(j = 0 ; j < p.nbSamplesMeasure ; ++j)
            //            {
            //                gsl_matrix_get_col(s.sigmaPointMeasure, s.sigmaPointsMeasure, j);
            //                h(s.sigmaPointMeasure, &gsl_matrix_column(s.yi_prediction,j).vector);
            //
            //                // Update the mean , eq (3.176)
            //                for(i = 0 ; i < p.no ; ++i)
            //                    yi->data[i] += s.wm_aug_j->data[j] * gsl_matrix_get(s.yi_prediction,i,j);
            //            }
        }
    } // srstate
} // ukf

#endif // UKF_SR_STATE_NDIM_H


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:43:59 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -real2hc 32 */

/*
 * This function contains 156 FP additions, 42 FP multiplications,
 * (or, 140 additions, 26 multiplications, 16 fused multiply/add),
 * 46 stack variables, and 64 memory accesses
 */
static const fftw_real K195090322 = FFTW_KONST(+0.195090322016128267848284868477022240927691618);
static const fftw_real K980785280 = FFTW_KONST(+0.980785280403230449126182236134239036973933731);
static const fftw_real K555570233 = FFTW_KONST(+0.555570233019602224742830813948532874374937191);
static const fftw_real K831469612 = FFTW_KONST(+0.831469612302545237078788377617905756738560812);
static const fftw_real K382683432 = FFTW_KONST(+0.382683432365089771728459984030398866761344562);
static const fftw_real K923879532 = FFTW_KONST(+0.923879532511286756128183189396788286822416626);
static const fftw_real K707106781 = FFTW_KONST(+0.707106781186547524400844362104849039284835938);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_real2hc_32(const fftw_real *input, fftw_real *real_output, fftw_real *imag_output, int istride, int real_ostride, int imag_ostride)
{
     fftw_real tmp7;
     fftw_real tmp135;
     fftw_real tmp31;
     fftw_real tmp83;
     fftw_real tmp14;
     fftw_real tmp148;
     fftw_real tmp34;
     fftw_real tmp82;
     fftw_real tmp123;
     fftw_real tmp143;
     fftw_real tmp71;
     fftw_real tmp99;
     fftw_real tmp126;
     fftw_real tmp144;
     fftw_real tmp76;
     fftw_real tmp100;
     fftw_real tmp22;
     fftw_real tmp136;
     fftw_real tmp38;
     fftw_real tmp80;
     fftw_real tmp29;
     fftw_real tmp137;
     fftw_real tmp41;
     fftw_real tmp79;
     fftw_real tmp116;
     fftw_real tmp140;
     fftw_real tmp54;
     fftw_real tmp96;
     fftw_real tmp119;
     fftw_real tmp141;
     fftw_real tmp59;
     fftw_real tmp97;
     ASSERT_ALIGNED_DOUBLE;
     {
	  fftw_real tmp1;
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  fftw_real tmp5;
	  fftw_real tmp6;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = input[0];
	  tmp2 = input[16 * istride];
	  tmp3 = tmp1 + tmp2;
	  tmp4 = input[8 * istride];
	  tmp5 = input[24 * istride];
	  tmp6 = tmp4 + tmp5;
	  tmp7 = tmp3 + tmp6;
	  tmp135 = tmp3 - tmp6;
	  tmp31 = tmp1 - tmp2;
	  tmp83 = tmp4 - tmp5;
     }
     {
	  fftw_real tmp10;
	  fftw_real tmp32;
	  fftw_real tmp13;
	  fftw_real tmp33;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp8;
	       fftw_real tmp9;
	       fftw_real tmp11;
	       fftw_real tmp12;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp8 = input[4 * istride];
	       tmp9 = input[20 * istride];
	       tmp10 = tmp8 + tmp9;
	       tmp32 = tmp8 - tmp9;
	       tmp11 = input[28 * istride];
	       tmp12 = input[12 * istride];
	       tmp13 = tmp11 + tmp12;
	       tmp33 = tmp11 - tmp12;
	  }
	  tmp14 = tmp10 + tmp13;
	  tmp148 = tmp13 - tmp10;
	  tmp34 = K707106781 * (tmp32 + tmp33);
	  tmp82 = K707106781 * (tmp33 - tmp32);
     }
     {
	  fftw_real tmp63;
	  fftw_real tmp121;
	  fftw_real tmp75;
	  fftw_real tmp122;
	  fftw_real tmp66;
	  fftw_real tmp124;
	  fftw_real tmp69;
	  fftw_real tmp125;
	  fftw_real tmp70;
	  fftw_real tmp72;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp61;
	       fftw_real tmp62;
	       fftw_real tmp73;
	       fftw_real tmp74;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp61 = input[31 * istride];
	       tmp62 = input[15 * istride];
	       tmp63 = tmp61 - tmp62;
	       tmp121 = tmp61 + tmp62;
	       tmp73 = input[7 * istride];
	       tmp74 = input[23 * istride];
	       tmp75 = tmp73 - tmp74;
	       tmp122 = tmp73 + tmp74;
	  }
	  {
	       fftw_real tmp64;
	       fftw_real tmp65;
	       fftw_real tmp67;
	       fftw_real tmp68;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp64 = input[3 * istride];
	       tmp65 = input[19 * istride];
	       tmp66 = tmp64 - tmp65;
	       tmp124 = tmp64 + tmp65;
	       tmp67 = input[27 * istride];
	       tmp68 = input[11 * istride];
	       tmp69 = tmp67 - tmp68;
	       tmp125 = tmp67 + tmp68;
	  }
	  tmp123 = tmp121 + tmp122;
	  tmp143 = tmp121 - tmp122;
	  tmp70 = K707106781 * (tmp66 + tmp69);
	  tmp71 = tmp63 + tmp70;
	  tmp99 = tmp63 - tmp70;
	  tmp126 = tmp124 + tmp125;
	  tmp144 = tmp125 - tmp124;
	  tmp72 = K707106781 * (tmp69 - tmp66);
	  tmp76 = tmp72 - tmp75;
	  tmp100 = tmp75 + tmp72;
     }
     {
	  fftw_real tmp18;
	  fftw_real tmp36;
	  fftw_real tmp21;
	  fftw_real tmp37;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp16;
	       fftw_real tmp17;
	       fftw_real tmp19;
	       fftw_real tmp20;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp16 = input[2 * istride];
	       tmp17 = input[18 * istride];
	       tmp18 = tmp16 + tmp17;
	       tmp36 = tmp16 - tmp17;
	       tmp19 = input[10 * istride];
	       tmp20 = input[26 * istride];
	       tmp21 = tmp19 + tmp20;
	       tmp37 = tmp19 - tmp20;
	  }
	  tmp22 = tmp18 + tmp21;
	  tmp136 = tmp18 - tmp21;
	  tmp38 = (K923879532 * tmp36) - (K382683432 * tmp37);
	  tmp80 = (K382683432 * tmp36) + (K923879532 * tmp37);
     }
     {
	  fftw_real tmp25;
	  fftw_real tmp39;
	  fftw_real tmp28;
	  fftw_real tmp40;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp23;
	       fftw_real tmp24;
	       fftw_real tmp26;
	       fftw_real tmp27;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp23 = input[30 * istride];
	       tmp24 = input[14 * istride];
	       tmp25 = tmp23 + tmp24;
	       tmp39 = tmp23 - tmp24;
	       tmp26 = input[6 * istride];
	       tmp27 = input[22 * istride];
	       tmp28 = tmp26 + tmp27;
	       tmp40 = tmp26 - tmp27;
	  }
	  tmp29 = tmp25 + tmp28;
	  tmp137 = tmp25 - tmp28;
	  tmp41 = (K923879532 * tmp39) + (K382683432 * tmp40);
	  tmp79 = (K382683432 * tmp39) - (K923879532 * tmp40);
     }
     {
	  fftw_real tmp46;
	  fftw_real tmp114;
	  fftw_real tmp58;
	  fftw_real tmp115;
	  fftw_real tmp49;
	  fftw_real tmp117;
	  fftw_real tmp52;
	  fftw_real tmp118;
	  fftw_real tmp53;
	  fftw_real tmp55;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp44;
	       fftw_real tmp45;
	       fftw_real tmp56;
	       fftw_real tmp57;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp44 = input[istride];
	       tmp45 = input[17 * istride];
	       tmp46 = tmp44 - tmp45;
	       tmp114 = tmp44 + tmp45;
	       tmp56 = input[9 * istride];
	       tmp57 = input[25 * istride];
	       tmp58 = tmp56 - tmp57;
	       tmp115 = tmp56 + tmp57;
	  }
	  {
	       fftw_real tmp47;
	       fftw_real tmp48;
	       fftw_real tmp50;
	       fftw_real tmp51;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp47 = input[5 * istride];
	       tmp48 = input[21 * istride];
	       tmp49 = tmp47 - tmp48;
	       tmp117 = tmp47 + tmp48;
	       tmp50 = input[29 * istride];
	       tmp51 = input[13 * istride];
	       tmp52 = tmp50 - tmp51;
	       tmp118 = tmp50 + tmp51;
	  }
	  tmp116 = tmp114 + tmp115;
	  tmp140 = tmp114 - tmp115;
	  tmp53 = K707106781 * (tmp49 + tmp52);
	  tmp54 = tmp46 + tmp53;
	  tmp96 = tmp46 - tmp53;
	  tmp119 = tmp117 + tmp118;
	  tmp141 = tmp118 - tmp117;
	  tmp55 = K707106781 * (tmp52 - tmp49);
	  tmp59 = tmp55 - tmp58;
	  tmp97 = tmp58 + tmp55;
     }
     {
	  fftw_real tmp15;
	  fftw_real tmp30;
	  fftw_real tmp131;
	  fftw_real tmp132;
	  fftw_real tmp133;
	  fftw_real tmp134;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp15 = tmp7 + tmp14;
	  tmp30 = tmp22 + tmp29;
	  tmp131 = tmp15 + tmp30;
	  tmp132 = tmp116 + tmp119;
	  tmp133 = tmp123 + tmp126;
	  tmp134 = tmp132 + tmp133;
	  real_output[8 * real_ostride] = tmp15 - tmp30;
	  imag_output[8 * imag_ostride] = tmp133 - tmp132;
	  real_output[16 * real_ostride] = tmp131 - tmp134;
	  real_output[0] = tmp131 + tmp134;
     }
     {
	  fftw_real tmp113;
	  fftw_real tmp129;
	  fftw_real tmp128;
	  fftw_real tmp130;
	  fftw_real tmp120;
	  fftw_real tmp127;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp113 = tmp7 - tmp14;
	  tmp129 = tmp29 - tmp22;
	  tmp120 = tmp116 - tmp119;
	  tmp127 = tmp123 - tmp126;
	  tmp128 = K707106781 * (tmp120 + tmp127);
	  tmp130 = K707106781 * (tmp127 - tmp120);
	  real_output[12 * real_ostride] = tmp113 - tmp128;
	  real_output[4 * real_ostride] = tmp113 + tmp128;
	  imag_output[4 * imag_ostride] = tmp129 + tmp130;
	  imag_output[12 * imag_ostride] = tmp130 - tmp129;
     }
     {
	  fftw_real tmp139;
	  fftw_real tmp155;
	  fftw_real tmp149;
	  fftw_real tmp151;
	  fftw_real tmp146;
	  fftw_real tmp150;
	  fftw_real tmp154;
	  fftw_real tmp156;
	  fftw_real tmp138;
	  fftw_real tmp147;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp138 = K707106781 * (tmp136 + tmp137);
	  tmp139 = tmp135 + tmp138;
	  tmp155 = tmp135 - tmp138;
	  tmp147 = K707106781 * (tmp137 - tmp136);
	  tmp149 = tmp147 - tmp148;
	  tmp151 = tmp148 + tmp147;
	  {
	       fftw_real tmp142;
	       fftw_real tmp145;
	       fftw_real tmp152;
	       fftw_real tmp153;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp142 = (K923879532 * tmp140) + (K382683432 * tmp141);
	       tmp145 = (K923879532 * tmp143) - (K382683432 * tmp144);
	       tmp146 = tmp142 + tmp145;
	       tmp150 = tmp145 - tmp142;
	       tmp152 = (K923879532 * tmp141) - (K382683432 * tmp140);
	       tmp153 = (K382683432 * tmp143) + (K923879532 * tmp144);
	       tmp154 = tmp152 + tmp153;
	       tmp156 = tmp153 - tmp152;
	  }
	  real_output[14 * real_ostride] = tmp139 - tmp146;
	  real_output[2 * real_ostride] = tmp139 + tmp146;
	  imag_output[6 * imag_ostride] = tmp149 + tmp150;
	  imag_output[10 * imag_ostride] = tmp150 - tmp149;
	  imag_output[2 * imag_ostride] = tmp151 + tmp154;
	  imag_output[14 * imag_ostride] = tmp154 - tmp151;
	  real_output[10 * real_ostride] = tmp155 - tmp156;
	  real_output[6 * real_ostride] = tmp155 + tmp156;
     }
     {
	  fftw_real tmp95;
	  fftw_real tmp111;
	  fftw_real tmp110;
	  fftw_real tmp112;
	  fftw_real tmp102;
	  fftw_real tmp106;
	  fftw_real tmp105;
	  fftw_real tmp107;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp93;
	       fftw_real tmp94;
	       fftw_real tmp108;
	       fftw_real tmp109;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp93 = tmp31 - tmp34;
	       tmp94 = tmp80 + tmp79;
	       tmp95 = tmp93 + tmp94;
	       tmp111 = tmp93 - tmp94;
	       tmp108 = (K831469612 * tmp97) - (K555570233 * tmp96);
	       tmp109 = (K555570233 * tmp99) + (K831469612 * tmp100);
	       tmp110 = tmp108 + tmp109;
	       tmp112 = tmp109 - tmp108;
	  }
	  {
	       fftw_real tmp98;
	       fftw_real tmp101;
	       fftw_real tmp103;
	       fftw_real tmp104;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp98 = (K831469612 * tmp96) + (K555570233 * tmp97);
	       tmp101 = (K831469612 * tmp99) - (K555570233 * tmp100);
	       tmp102 = tmp98 + tmp101;
	       tmp106 = tmp101 - tmp98;
	       tmp103 = tmp41 - tmp38;
	       tmp104 = tmp83 + tmp82;
	       tmp105 = tmp103 - tmp104;
	       tmp107 = tmp104 + tmp103;
	  }
	  real_output[13 * real_ostride] = tmp95 - tmp102;
	  real_output[3 * real_ostride] = tmp95 + tmp102;
	  imag_output[5 * imag_ostride] = tmp105 + tmp106;
	  imag_output[11 * imag_ostride] = tmp106 - tmp105;
	  imag_output[3 * imag_ostride] = tmp107 + tmp110;
	  imag_output[13 * imag_ostride] = tmp110 - tmp107;
	  real_output[11 * real_ostride] = tmp111 - tmp112;
	  real_output[5 * real_ostride] = tmp111 + tmp112;
     }
     {
	  fftw_real tmp43;
	  fftw_real tmp91;
	  fftw_real tmp90;
	  fftw_real tmp92;
	  fftw_real tmp78;
	  fftw_real tmp86;
	  fftw_real tmp85;
	  fftw_real tmp87;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp35;
	       fftw_real tmp42;
	       fftw_real tmp88;
	       fftw_real tmp89;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp35 = tmp31 + tmp34;
	       tmp42 = tmp38 + tmp41;
	       tmp43 = tmp35 + tmp42;
	       tmp91 = tmp35 - tmp42;
	       tmp88 = (K980785280 * tmp59) - (K195090322 * tmp54);
	       tmp89 = (K195090322 * tmp71) + (K980785280 * tmp76);
	       tmp90 = tmp88 + tmp89;
	       tmp92 = tmp89 - tmp88;
	  }
	  {
	       fftw_real tmp60;
	       fftw_real tmp77;
	       fftw_real tmp81;
	       fftw_real tmp84;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp60 = (K980785280 * tmp54) + (K195090322 * tmp59);
	       tmp77 = (K980785280 * tmp71) - (K195090322 * tmp76);
	       tmp78 = tmp60 + tmp77;
	       tmp86 = tmp77 - tmp60;
	       tmp81 = tmp79 - tmp80;
	       tmp84 = tmp82 - tmp83;
	       tmp85 = tmp81 - tmp84;
	       tmp87 = tmp84 + tmp81;
	  }
	  real_output[15 * real_ostride] = tmp43 - tmp78;
	  real_output[real_ostride] = tmp43 + tmp78;
	  imag_output[7 * imag_ostride] = tmp85 + tmp86;
	  imag_output[9 * imag_ostride] = tmp86 - tmp85;
	  imag_output[imag_ostride] = tmp87 + tmp90;
	  imag_output[15 * imag_ostride] = tmp90 - tmp87;
	  real_output[9 * real_ostride] = tmp91 - tmp92;
	  real_output[7 * real_ostride] = tmp91 + tmp92;
     }
}

fftw_codelet_desc fftw_real2hc_32_desc =
{
     "fftw_real2hc_32",
     (void (*)()) fftw_real2hc_32,
     32,
     FFTW_FORWARD,
     FFTW_REAL2HC,
     706,
     0,
     (const int *) 0,
};


#ifndef GBNET_MODELORNOR
#define GBNET_MODELORNOR

#include <cmath>
#include <time.h>

#include <gsl/gsl_rstat.h>

#include "ModelBase.h"
#include "GraphORNOR.h"

namespace gbn
{

    class ModelORNOR: public ModelBase
    {
        private:
        protected:
        public:
            ModelORNOR();
            ~ModelORNOR() override;
            ModelORNOR(const network_t, const evidence_dict_t, const prior_active_tf_set_t = prior_active_tf_set_t(),
                       const double [3 * 3] = gbn::SPRIOR, double z_alpha = 25., double z_beta = 25., double z0_alpha = 25., double z0_beta = 25., double t_alpha = 25., double t_beta = 25.,
                       unsigned int = 3, bool = true, bool = true, bool = false,
                       double = 2., double = 2., double = 2., double = 2., double = 8., double = 2.);
    };
}

#endif


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/* funcs.h */

#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

#define WS_SZ	1000  			/* size of the integration workspace */

#define POW_2	gsl_pow_2  		/* square */
#define VECTOR	gsl_vector_get		/* vector_get */

struct f_params { double omega_c ; double beta ; double Omega ;
	double omega_1; double alpha; } ;
int assign_p ( void*, double*, double*, double*, double* ) ;

struct ent_pars { const gsl_vector* rhoeq ; gsl_matrix* red_m ; } ;

/* Re_gsc functions */
double k_func ( double, void* ) ;
double k_func_1 ( double , void* ) ;
double k_func_2 ( double , void* ) ;
double k_func_3 ( double , void* ) ;
double k_func_4 ( double , void* ) ;
int first_int ( double*, double*, void* ) ;
int second_int ( double* , double* , void* ) ;
int third_int ( double* , double* , void* ) ;
int fourth_int ( double* , double* , void* ) ;
int re_gsc ( double* result, double* error, void* params ) ;
int re_gcs ( double* result, double* error, void* params ) ;

int re_gcc ( void* params, double* val ) ;
int im_gcc ( void* params, double* val, double* error ) ;
int re_gss ( void* params, double* val ) ;
int im_gss ( void* params, double* val, double* error ) ;
int im_gcs ( void* params, double* val ) ;
int im_gsc ( void* params, double* val ) ;
int re_gc0 ( void* params, double* val ) ;
int im_gc0 ( void* params, double* val, double* error ) ;
int re_gs0 ( void* params, double* val, double* error ) ;
int im_gs0 ( void* params, double* val ) ;

/* Img_c0, Img_cc, Img_ss, Img_cs functions */
int expi ( double , double*, double* ) ;
int expi_plus ( double, double*, double* ) ;
double fu ( double, void* ) ;
double ex ( double, void* ) ;

/* Reg_s0 functions */
double fu_inv ( double, void* ) ;
double fu_cau ( double, void* ) ;
double fu_coth ( double, void* ) ;

/* Functions to create matrices */
int red_mat ( gsl_matrix* , void* ) ;
int cp_mat ( gsl_matrix*, void* ) ;
int ham_gen ( gsl_matrix*, const double* ) ;
int mat_write ( gsl_matrix*, char* ) ;
int mat_read ( gsl_matrix*, char* ) ;
int integration ( void*, double* ) ;

/* Functions for evolution */
int generator ( double, const double*, double*, void* ) ;
int jac ( double, const double*, double*, double*, void* ) ;
int evol ( double, gsl_vector*, double, gsl_odeiv2_evolve*,
		gsl_odeiv2_control*, gsl_odeiv2_step*, gsl_odeiv2_system* ) ;
int red_evol ( void*, const double*, double, double, const gsl_vector*,	gsl_matrix* ) ;
int cp_evol ( void*, const double*, double, double, const gsl_vector*,	gsl_matrix* ) ;

/* Stationarity */
int stationary ( const gsl_matrix* , gsl_vector*  ) ;
double J ( double, double ) ;
double polarization ( void*, double ) ;

/* Entropy */
double entropy_of_state ( const gsl_vector* ) ;
double entropy_production ( const gsl_vector*, const gsl_vector*, const gsl_matrix* ) ;
double entropy_prod_time  ( double, void* ) ;
double entropy_integ ( double, const gsl_vector*, gsl_matrix* ) ;

/* DC current */
double current ( const gsl_vector* ) ;
double current_temp ( double ) ;

/* total current */
double tot_current ( const gsl_vector*, void* ) ;

/* writing operations */
int save_integrals ( void* ) ;
int save_matrices ( void* ) ;
int write_red_curr_T ( void* ) ;
int write_cp_curr_T ( void* ) ;
int write_red_curr_O ( void* ) ;
int write_cp_curr_O ( void* ) ;

/* currents */
double current_red_om ( double, void* ) ;
double current_red_T ( double, void* ) ;
double current_cp_om ( double, void* ) ;
double current_cp_T ( double, void* ) ;

/* positivity */
double r0_dot ( const gsl_matrix* , const gsl_vector*  ) ;
int bloch_vector ( gsl_vector* , double , double , double ) ;
int polars ( double* , double* , double* , const gsl_vector*  ) ;
int sample ( const gsl_matrix* , unsigned int ) ;

/* posit */
gsl_matrix_complex* state_matrix ( double, double ) ;
gsl_matrix_complex* rotate ( gsl_matrix_complex*, double ) ;
gsl_matrix_complex* D_psi ( gsl_matrix_complex* ) ;

/* pos_study */
double delta ( double, double*, double, double, double ) ;
int read_integs ( double* ) ;

/* entropy production with different parameters */
int ent_prod_fixed_t ( double, double ) ;
int ent_prod_red_evol ( void* , const double* , double , double , const gsl_vector* , gsl_matrix* , double ) ;
char* dtoascii ( double, int ) ;


#pragma once

#ifndef _NOGSL
#include <gsl/gsl_vector.h>
#else
#include "CustomSolver.h"
#endif
#include "ValueGrad.h"
#include "BooleanNodes.h"
#include "BooleanDAG.h"
#include "NodeVisitor.h"
#include "VarStore.h"
#include <map>
#include <set>
#include "SymbolicEvaluator.h"
#include "Interface.h"

#include <iostream>

using namespace std;

class BoolAutoDiff: public NodeVisitor, public SymbolicEvaluator
{
	BooleanDAG& bdag;
	map<string, int>& floatCtrls; // Maps float ctrl names to indices within grad vector
	int nctrls; // number of float ctrls
	gsl_vector* ctrls; // ctrl values
	vector<ValueGrad*> values; // Keeps track of values along with gradients for each node
	vector<DistanceGrad*> distances; // Keeps track of distance metric for boolean nodes
    Interface* inputValues;

public:
    int DEFAULT_INP = -32;
    BoolAutoDiff(BooleanDAG& bdag_p, map<string, int>& floatCtrls_p);

	~BoolAutoDiff(void);

	virtual void visit( SRC_node& node );
	virtual void visit( DST_node& node );
	virtual void visit( CTRL_node& node );
	virtual void visit( PLUS_node& node );
	virtual void visit( TIMES_node& node );
	virtual void visit( ARRACC_node& node );
	virtual void visit( DIV_node& node );
	virtual void visit( MOD_node& node );
	virtual void visit( NEG_node& node );
	virtual void visit( CONST_node& node );
	virtual void visit( LT_node& node );
	virtual void visit( EQ_node& node );
	virtual void visit( AND_node& node );
	virtual void visit( OR_node& node );
	virtual void visit( NOT_node& node );
	virtual void visit( ARRASS_node& node );
	virtual void visit( UFUN_node& node );
	virtual void visit( TUPLE_R_node& node );
	virtual void visit( ASSERT_node& node );

    virtual void setInputs(Interface* inputValues_p);

	virtual void run(const gsl_vector* ctrls_p, const set<int>& nodesSubset);
    virtual void run(const gsl_vector* ctrls_p);

    virtual double getErrorOnConstraint(int nodeid, gsl_vector* grad);
    double getSqrtError(bool_node* node, gsl_vector* grad);
    double getAssertError(bool_node* node, gsl_vector* grad);
    double getBoolCtrlError(bool_node* node, gsl_vector* grad);
    double getBoolExprError(bool_node* node, gsl_vector* grad);

    virtual double getErrorOnConstraint(int nodeid);
    double getSqrtError(bool_node* node);
    double getAssertError(bool_node* node);
    double getBoolCtrlError(bool_node* node);
    double getBoolExprError(bool_node* node);

    virtual double getErrorForAsserts(const set<int>& assertIds, gsl_vector* grad);
	virtual double getErrorForAssert(int assertId, gsl_vector* grad);

    double computeSingleError(int nodeid, double errorSoFar, const gsl_vector* errorGradSoFar, gsl_vector* grad);

	void setvalue(bool_node& bn, ValueGrad* v) {
		values[bn.id] = v;
	}

	ValueGrad* v(bool_node& bn) {
		ValueGrad* val = values[bn.id];
		if (val == NULL) {
			gsl_vector* g = gsl_vector_alloc(nctrls);
			val = new ValueGrad(0, g);
			setvalue(bn, val);
		}
		return val;
	}

	ValueGrad* v(bool_node* bn) {
		return v(*bn);
	}

	void setdistance(bool_node& bn, DistanceGrad* d) {
		distances[bn.id] = d;
	}

	DistanceGrad* d(bool_node& bn) {
		DistanceGrad* dist = distances[bn.id];
		if (dist == NULL) {
			gsl_vector* g = gsl_vector_alloc(nctrls);
			dist = new DistanceGrad(0, g);
			setdistance(bn, dist);
		}
		return dist;
	}

	DistanceGrad* d(bool_node* bn) {
		return d(*bn);
	}

	virtual double dist(int nid) {
		DistanceGrad* dg = d(bdag[nid]);
		return dg->dist;
	}

	virtual double dist(int nid, gsl_vector* grad) {
		DistanceGrad* dg = d(bdag[nid]);
		gsl_vector_memcpy(grad, dg->grad);
		return dg->dist;
	}

	virtual void print() {
		/*for (int i = 0; i < bdag.size(); i++) {
			if (bdag[i]->type == bool_node::ASSERT) {
				DistanceGrad* dist = d(bdag[i]->mother);
				if (dist->set) {
				double gmag = gsl_blas_dnrm2(dist->grad);
				if (dist->dist < 0.1) {
					cout << bdag[i]->mother->lprint() << endl;
					cout << dist->printFull() << endl;
				}
				}
			}
		}
		return;*/
		for (int i = 0; i < bdag.size(); i++) {
			cout << bdag[i]->lprint() << " ";
			if (bdag[i]->getOtype() == OutType::FLOAT) {
				if (v(bdag[i])->set) {
					cout << v(bdag[i])->print() << endl;
				} else {
					cout << "UNSET" << endl;
				}
			} else {
				if (d(bdag[i])->set) {
					cout << d(bdag[i])->print() << endl;
				} else {
					cout << "UNSET" << endl;
				}
			}
		}
	}
	virtual void printFull() {
		for (int i = 0; i < bdag.size(); i++) {
			cout << bdag[i]->lprint() << endl;
			if (bdag[i]->getOtype() == OutType::FLOAT) {
				if (v(bdag[i])->set) {
					cout << v(bdag[i])->printFull() << endl;
				} else {
					cout << "UNSET" <<endl;
				}
			} else {
				if(d(bdag[i])->set) {
					cout << d(bdag[i])->printFull() << endl;
				} else {
					cout << "UNSET" << endl;
				}
			}
			if (bdag[i]->type == bool_node::PLUS) {
				cout << v(bdag[i]->mother())->getVal() << " + " << v(bdag[i]->father())->getVal() << endl;
			}
		}
	}

	bool isFloat(bool_node& bn) {
		return (bn.getOtype() == OutType::FLOAT);
	}

	bool isFloat(bool_node* bn) {
		return (bn->getOtype() == OutType::FLOAT);
	}

	int getInputValue(bool_node& bn) {
		if (inputValues->hasValue(bn.id)) {
			int val = inputValues->getValue(bn.id);
			return val;
		} else {
			return DEFAULT_INP;
		}
	}

	int getInputValue(bool_node* bn) {
		return getInputValue(*bn);
	}

};


/* statistics/test_nist.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Jim Davies, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* These tests are based on the NIST Statistical Reference Datasets
   See http://www.nist.gov/itl/div898/strd/index.html for more
   information. */

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

#include <gsl/gsl_test.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_ieee_utils.h>

int
test_nist (void)
{
  size_t i ;

  const size_t nlew = 200 ;

  const double lew[200] = {
    -213, -564,  -35,  -15,  141,  115, -420, -360,  203, -338, -431,  194,
    -220, -513,  154, -125, -559,   92,  -21, -579,  -52,   99, -543, -175,
     162, -457, -346,  204, -300, -474,  164, -107, -572,   -8,   83, -541,
    -224,  180, -420, -374,  201, -236, -531,   83,   27, -564, -112,  131,
    -507, -254,  199, -311, -495,  143,  -46, -579,  -90,  136, -472, -338,
     202, -287, -477,  169, -124, -568,   17,   48, -568, -135,  162, -430,
    -422,  172,  -74, -577,  -13,   92, -534, -243,  194, -355, -465,  156,
     -81, -578,  -64,  139, -449, -384,  193, -198, -538,  110,  -44, -577,
      -6,   66, -552, -164,  161, -460, -344,  205, -281, -504,  134,  -28,
    -576, -118,  156, -437, -381,  200, -220, -540,   83,   11, -568, -160,
     172, -414, -408,  188, -125, -572,  -32,  139, -492, -321,  205, -262,
    -504,  142,  -83, -574,    0,   48, -571, -106,  137, -501, -266,  190,
    -391, -406,  194, -186, -553,   83,  -13, -577,  -49,  103, -515, -280,
     201,  300, -506,  131,  -45, -578,  -80,  138, -462, -361,  201, -211,
    -554,   32,   74, -533, -235,  187, -372, -442,  182, -147, -566,   25,
      68, -535, -244,  194, -351, -463,  174, -125, -570,   15,   72, -550,
    -190,  172, -424, -385,  198, -218, -536,   96 } ;

  const size_t nlottery = 218 ;

  const double lottery[218] = {
    162, 671, 933, 414, 788, 730, 817, 33, 536, 875, 670, 236, 473, 167,
    877, 980, 316, 950, 456, 92, 517, 557, 956, 954, 104, 178, 794, 278,
    147, 773, 437, 435, 502, 610, 582, 780, 689, 562, 964, 791, 28, 97,
    848, 281, 858, 538, 660, 972, 671, 613, 867, 448, 738, 966, 139, 636,
    847, 659, 754, 243, 122, 455, 195, 968, 793, 59, 730, 361, 574, 522,
    97, 762, 431, 158, 429, 414, 22, 629, 788, 999, 187, 215, 810, 782,
    47, 34, 108, 986, 25, 644, 829, 630, 315, 567, 919, 331, 207, 412,
    242, 607, 668, 944, 749, 168, 864, 442, 533, 805, 372, 63, 458, 777,
    416, 340, 436, 140, 919, 350, 510, 572, 905, 900, 85, 389, 473, 758,
    444, 169, 625, 692, 140, 897, 672, 288, 312, 860, 724, 226, 884, 508,
    976, 741, 476, 417, 831, 15, 318, 432, 241, 114, 799, 955, 833, 358,
    935, 146, 630, 830, 440, 642, 356, 373, 271, 715, 367, 393, 190, 669,
    8, 861, 108, 795, 269, 590, 326, 866, 64, 523, 862, 840, 219, 382,
    998, 4, 628, 305, 747, 247, 34, 747, 729, 645, 856, 974, 24, 568, 24,
    694, 608, 480, 410, 729, 947, 293, 53, 930, 223, 203, 677, 227, 62,
    455, 387, 318, 562, 242, 428, 968 } ;

  const size_t nmavro = 50 ;

  const double mavro[50] = {
    2.00180, 2.00170, 2.00180, 2.00190, 2.00180, 2.00170, 2.00150,
    2.00140, 2.00150, 2.00150, 2.00170, 2.00180, 2.00180, 2.00190,
    2.00190, 2.00210, 2.00200, 2.00160, 2.00140, 2.00130, 2.00130,
    2.00150, 2.00150, 2.00160, 2.00150, 2.00140, 2.00130, 2.00140,
    2.00150, 2.00140, 2.00150, 2.00160, 2.00150, 2.00160, 2.00190,
    2.00200, 2.00200, 2.00210, 2.00220, 2.00230, 2.00240, 2.00250,
    2.00270, 2.00260, 2.00260, 2.00260, 2.00270, 2.00260, 2.00250,
    2.00240 } ;

  const size_t nmichelson = 100 ;

  const double michelson [100] = {
    299.85, 299.74, 299.90, 300.07, 299.93, 299.85, 299.95, 299.98,
    299.98, 299.88, 300.00, 299.98, 299.93, 299.65, 299.76, 299.81,
    300.00, 300.00, 299.96, 299.96, 299.96, 299.94, 299.96, 299.94,
    299.88, 299.80, 299.85, 299.88, 299.90, 299.84, 299.83, 299.79,
    299.81, 299.88, 299.88, 299.83, 299.80, 299.79, 299.76, 299.80,
    299.88, 299.88, 299.88, 299.86, 299.72, 299.72, 299.62, 299.86,
    299.97, 299.95, 299.88, 299.91, 299.85, 299.87, 299.84, 299.84,
    299.85, 299.84, 299.84, 299.84, 299.89, 299.81, 299.81, 299.82,
    299.80, 299.77, 299.76, 299.74, 299.75, 299.76, 299.91, 299.92,
    299.89, 299.86, 299.88, 299.72, 299.84, 299.85, 299.85, 299.78,
    299.89, 299.84, 299.78, 299.81, 299.76, 299.81, 299.79, 299.81,
    299.82, 299.85, 299.87, 299.87, 299.81, 299.74, 299.81, 299.94,
    299.95, 299.80, 299.81, 299.87 } ;

  const size_t npidigits = 5000 ;
  const double pidigits [5000] = { 3, 1, 4, 1, 5, 9, 2, 6, 5, 3, 5, 8,
    9, 7, 9, 3, 2, 3, 8, 4, 6, 2, 6, 4, 3, 3, 8, 3, 2, 7, 9, 5, 0, 2,
    8, 8, 4, 1, 9, 7, 1, 6, 9, 3, 9, 9, 3, 7, 5, 1, 0, 5, 8, 2, 0, 9,
    7, 4, 9, 4, 4, 5, 9, 2, 3, 0, 7, 8, 1, 6, 4, 0, 6, 2, 8, 6, 2, 0,
    8, 9, 9, 8, 6, 2, 8, 0, 3, 4, 8, 2, 5, 3, 4, 2, 1, 1, 7, 0, 6, 7,
    9, 8, 2, 1, 4, 8, 0, 8, 6, 5, 1, 3, 2, 8, 2, 3, 0, 6, 6, 4, 7, 0,
    9, 3, 8, 4, 4, 6, 0, 9, 5, 5, 0, 5, 8, 2, 2, 3, 1, 7, 2, 5, 3, 5,
    9, 4, 0, 8, 1, 2, 8, 4, 8, 1, 1, 1, 7, 4, 5, 0, 2, 8, 4, 1, 0, 2,
    7, 0, 1, 9, 3, 8, 5, 2, 1, 1, 0, 5, 5, 5, 9, 6, 4, 4, 6, 2, 2, 9,
    4, 8, 9, 5, 4, 9, 3, 0, 3, 8, 1, 9, 6, 4, 4, 2, 8, 8, 1, 0, 9, 7,
    5, 6, 6, 5, 9, 3, 3, 4, 4, 6, 1, 2, 8, 4, 7, 5, 6, 4, 8, 2, 3, 3,
    7, 8, 6, 7, 8, 3, 1, 6, 5, 2, 7, 1, 2, 0, 1, 9, 0, 9, 1, 4, 5, 6,
    4, 8, 5, 6, 6, 9, 2, 3, 4, 6, 0, 3, 4, 8, 6, 1, 0, 4, 5, 4, 3, 2,
    6, 6, 4, 8, 2, 1, 3, 3, 9, 3, 6, 0, 7, 2, 6, 0, 2, 4, 9, 1, 4, 1,
    2, 7, 3, 7, 2, 4, 5, 8, 7, 0, 0, 6, 6, 0, 6, 3, 1, 5, 5, 8, 8, 1,
    7, 4, 8, 8, 1, 5, 2, 0, 9, 2, 0, 9, 6, 2, 8, 2, 9, 2, 5, 4, 0, 9,
    1, 7, 1, 5, 3, 6, 4, 3, 6, 7, 8, 9, 2, 5, 9, 0, 3, 6, 0, 0, 1, 1,
    3, 3, 0, 5, 3, 0, 5, 4, 8, 8, 2, 0, 4, 6, 6, 5, 2, 1, 3, 8, 4, 1,
    4, 6, 9, 5, 1, 9, 4, 1, 5, 1, 1, 6, 0, 9, 4, 3, 3, 0, 5, 7, 2, 7,
    0, 3, 6, 5, 7, 5, 9, 5, 9, 1, 9, 5, 3, 0, 9, 2, 1, 8, 6, 1, 1, 7,
    3, 8, 1, 9, 3, 2, 6, 1, 1, 7, 9, 3, 1, 0, 5, 1, 1, 8, 5, 4, 8, 0,
    7, 4, 4, 6, 2, 3, 7, 9, 9, 6, 2, 7, 4, 9, 5, 6, 7, 3, 5, 1, 8, 8,
    5, 7, 5, 2, 7, 2, 4, 8, 9, 1, 2, 2, 7, 9, 3, 8, 1, 8, 3, 0, 1, 1,
    9, 4, 9, 1, 2, 9, 8, 3, 3, 6, 7, 3, 3, 6, 2, 4, 4, 0, 6, 5, 6, 6,
    4, 3, 0, 8, 6, 0, 2, 1, 3, 9, 4, 9, 4, 6, 3, 9, 5, 2, 2, 4, 7, 3,
    7, 1, 9, 0, 7, 0, 2, 1, 7, 9, 8, 6, 0, 9, 4, 3, 7, 0, 2, 7, 7, 0,
    5, 3, 9, 2, 1, 7, 1, 7, 6, 2, 9, 3, 1, 7, 6, 7, 5, 2, 3, 8, 4, 6,
    7, 4, 8, 1, 8, 4, 6, 7, 6, 6, 9, 4, 0, 5, 1, 3, 2, 0, 0, 0, 5, 6,
    8, 1, 2, 7, 1, 4, 5, 2, 6, 3, 5, 6, 0, 8, 2, 7, 7, 8, 5, 7, 7, 1,
    3, 4, 2, 7, 5, 7, 7, 8, 9, 6, 0, 9, 1, 7, 3, 6, 3, 7, 1, 7, 8, 7,
    2, 1, 4, 6, 8, 4, 4, 0, 9, 0, 1, 2, 2, 4, 9, 5, 3, 4, 3, 0, 1, 4,
    6, 5, 4, 9, 5, 8, 5, 3, 7, 1, 0, 5, 0, 7, 9, 2, 2, 7, 9, 6, 8, 9,
    2, 5, 8, 9, 2, 3, 5, 4, 2, 0, 1, 9, 9, 5, 6, 1, 1, 2, 1, 2, 9, 0,
    2, 1, 9, 6, 0, 8, 6, 4, 0, 3, 4, 4, 1, 8, 1, 5, 9, 8, 1, 3, 6, 2,
    9, 7, 7, 4, 7, 7, 1, 3, 0, 9, 9, 6, 0, 5, 1, 8, 7, 0, 7, 2, 1, 1,
    3, 4, 9, 9, 9, 9, 9, 9, 8, 3, 7, 2, 9, 7, 8, 0, 4, 9, 9, 5, 1, 0,
    5, 9, 7, 3, 1, 7, 3, 2, 8, 1, 6, 0, 9, 6, 3, 1, 8, 5, 9, 5, 0, 2,
    4, 4, 5, 9, 4, 5, 5, 3, 4, 6, 9, 0, 8, 3, 0, 2, 6, 4, 2, 5, 2, 2,
    3, 0, 8, 2, 5, 3, 3, 4, 4, 6, 8, 5, 0, 3, 5, 2, 6, 1, 9, 3, 1, 1,
    8, 8, 1, 7, 1, 0, 1, 0, 0, 0, 3, 1, 3, 7, 8, 3, 8, 7, 5, 2, 8, 8,
    6, 5, 8, 7, 5, 3, 3, 2, 0, 8, 3, 8, 1, 4, 2, 0, 6, 1, 7, 1, 7, 7,
    6, 6, 9, 1, 4, 7, 3, 0, 3, 5, 9, 8, 2, 5, 3, 4, 9, 0, 4, 2, 8, 7,
    5, 5, 4, 6, 8, 7, 3, 1, 1, 5, 9, 5, 6, 2, 8, 6, 3, 8, 8, 2, 3, 5,
    3, 7, 8, 7, 5, 9, 3, 7, 5, 1, 9, 5, 7, 7, 8, 1, 8, 5, 7, 7, 3, 0,
    5, 3, 2, 1, 7, 1, 2, 2, 6, 8, 0, 6, 6, 1, 3, 0, 0, 1, 9, 2, 7, 8,
    7, 6, 6, 1, 1, 1, 9, 5, 9, 0, 9, 2, 1, 6, 4, 2, 0, 1, 9, 8, 9, 3,
    8, 0, 9, 5, 2, 5, 7, 2, 0, 1, 0, 6, 5, 4, 8, 5, 8, 6, 3, 2, 7, 8,
    8, 6, 5, 9, 3, 6, 1, 5, 3, 3, 8, 1, 8, 2, 7, 9, 6, 8, 2, 3, 0, 3,
    0, 1, 9, 5, 2, 0, 3, 5, 3, 0, 1, 8, 5, 2, 9, 6, 8, 9, 9, 5, 7, 7,
    3, 6, 2, 2, 5, 9, 9, 4, 1, 3, 8, 9, 1, 2, 4, 9, 7, 2, 1, 7, 7, 5,
    2, 8, 3, 4, 7, 9, 1, 3, 1, 5, 1, 5, 5, 7, 4, 8, 5, 7, 2, 4, 2, 4,
    5, 4, 1, 5, 0, 6, 9, 5, 9, 5, 0, 8, 2, 9, 5, 3, 3, 1, 1, 6, 8, 6,
    1, 7, 2, 7, 8, 5, 5, 8, 8, 9, 0, 7, 5, 0, 9, 8, 3, 8, 1, 7, 5, 4,
    6, 3, 7, 4, 6, 4, 9, 3, 9, 3, 1, 9, 2, 5, 5, 0, 6, 0, 4, 0, 0, 9,
    2, 7, 7, 0, 1, 6, 7, 1, 1, 3, 9, 0, 0, 9, 8, 4, 8, 8, 2, 4, 0, 1,
    2, 8, 5, 8, 3, 6, 1, 6, 0, 3, 5, 6, 3, 7, 0, 7, 6, 6, 0, 1, 0, 4,
    7, 1, 0, 1, 8, 1, 9, 4, 2, 9, 5, 5, 5, 9, 6, 1, 9, 8, 9, 4, 6, 7,
    6, 7, 8, 3, 7, 4, 4, 9, 4, 4, 8, 2, 5, 5, 3, 7, 9, 7, 7, 4, 7, 2,
    6, 8, 4, 7, 1, 0, 4, 0, 4, 7, 5, 3, 4, 6, 4, 6, 2, 0, 8, 0, 4, 6,
    6, 8, 4, 2, 5, 9, 0, 6, 9, 4, 9, 1, 2, 9, 3, 3, 1, 3, 6, 7, 7, 0,
    2, 8, 9, 8, 9, 1, 5, 2, 1, 0, 4, 7, 5, 2, 1, 6, 2, 0, 5, 6, 9, 6,
    6, 0, 2, 4, 0, 5, 8, 0, 3, 8, 1, 5, 0, 1, 9, 3, 5, 1, 1, 2, 5, 3,
    3, 8, 2, 4, 3, 0, 0, 3, 5, 5, 8, 7, 6, 4, 0, 2, 4, 7, 4, 9, 6, 4,
    7, 3, 2, 6, 3, 9, 1, 4, 1, 9, 9, 2, 7, 2, 6, 0, 4, 2, 6, 9, 9, 2,
    2, 7, 9, 6, 7, 8, 2, 3, 5, 4, 7, 8, 1, 6, 3, 6, 0, 0, 9, 3, 4, 1,
    7, 2, 1, 6, 4, 1, 2, 1, 9, 9, 2, 4, 5, 8, 6, 3, 1, 5, 0, 3, 0, 2,
    8, 6, 1, 8, 2, 9, 7, 4, 5, 5, 5, 7, 0, 6, 7, 4, 9, 8, 3, 8, 5, 0,
    5, 4, 9, 4, 5, 8, 8, 5, 8, 6, 9, 2, 6, 9, 9, 5, 6, 9, 0, 9, 2, 7,
    2, 1, 0, 7, 9, 7, 5, 0, 9, 3, 0, 2, 9, 5, 5, 3, 2, 1, 1, 6, 5, 3,
    4, 4, 9, 8, 7, 2, 0, 2, 7, 5, 5, 9, 6, 0, 2, 3, 6, 4, 8, 0, 6, 6,
    5, 4, 9, 9, 1, 1, 9, 8, 8, 1, 8, 3, 4, 7, 9, 7, 7, 5, 3, 5, 6, 6,
    3, 6, 9, 8, 0, 7, 4, 2, 6, 5, 4, 2, 5, 2, 7, 8, 6, 2, 5, 5, 1, 8,
    1, 8, 4, 1, 7, 5, 7, 4, 6, 7, 2, 8, 9, 0, 9, 7, 7, 7, 7, 2, 7, 9,
    3, 8, 0, 0, 0, 8, 1, 6, 4, 7, 0, 6, 0, 0, 1, 6, 1, 4, 5, 2, 4, 9,
    1, 9, 2, 1, 7, 3, 2, 1, 7, 2, 1, 4, 7, 7, 2, 3, 5, 0, 1, 4, 1, 4,
    4, 1, 9, 7, 3, 5, 6, 8, 5, 4, 8, 1, 6, 1, 3, 6, 1, 1, 5, 7, 3, 5,
    2, 5, 5, 2, 1, 3, 3, 4, 7, 5, 7, 4, 1, 8, 4, 9, 4, 6, 8, 4, 3, 8,
    5, 2, 3, 3, 2, 3, 9, 0, 7, 3, 9, 4, 1, 4, 3, 3, 3, 4, 5, 4, 7, 7,
    6, 2, 4, 1, 6, 8, 6, 2, 5, 1, 8, 9, 8, 3, 5, 6, 9, 4, 8, 5, 5, 6,
    2, 0, 9, 9, 2, 1, 9, 2, 2, 2, 1, 8, 4, 2, 7, 2, 5, 5, 0, 2, 5, 4,
    2, 5, 6, 8, 8, 7, 6, 7, 1, 7, 9, 0, 4, 9, 4, 6, 0, 1, 6, 5, 3, 4,
    6, 6, 8, 0, 4, 9, 8, 8, 6, 2, 7, 2, 3, 2, 7, 9, 1, 7, 8, 6, 0, 8,
    5, 7, 8, 4, 3, 8, 3, 8, 2, 7, 9, 6, 7, 9, 7, 6, 6, 8, 1, 4, 5, 4,
    1, 0, 0, 9, 5, 3, 8, 8, 3, 7, 8, 6, 3, 6, 0, 9, 5, 0, 6, 8, 0, 0,
    6, 4, 2, 2, 5, 1, 2, 5, 2, 0, 5, 1, 1, 7, 3, 9, 2, 9, 8, 4, 8, 9,
    6, 0, 8, 4, 1, 2, 8, 4, 8, 8, 6, 2, 6, 9, 4, 5, 6, 0, 4, 2, 4, 1,
    9, 6, 5, 2, 8, 5, 0, 2, 2, 2, 1, 0, 6, 6, 1, 1, 8, 6, 3, 0, 6, 7,
    4, 4, 2, 7, 8, 6, 2, 2, 0, 3, 9, 1, 9, 4, 9, 4, 5, 0, 4, 7, 1, 2,
    3, 7, 1, 3, 7, 8, 6, 9, 6, 0, 9, 5, 6, 3, 6, 4, 3, 7, 1, 9, 1, 7,
    2, 8, 7, 4, 6, 7, 7, 6, 4, 6, 5, 7, 5, 7, 3, 9, 6, 2, 4, 1, 3, 8,
    9, 0, 8, 6, 5, 8, 3, 2, 6, 4, 5, 9, 9, 5, 8, 1, 3, 3, 9, 0, 4, 7,
    8, 0, 2, 7, 5, 9, 0, 0, 9, 9, 4, 6, 5, 7, 6, 4, 0, 7, 8, 9, 5, 1,
    2, 6, 9, 4, 6, 8, 3, 9, 8, 3, 5, 2, 5, 9, 5, 7, 0, 9, 8, 2, 5, 8,
    2, 2, 6, 2, 0, 5, 2, 2, 4, 8, 9, 4, 0, 7, 7, 2, 6, 7, 1, 9, 4, 7,
    8, 2, 6, 8, 4, 8, 2, 6, 0, 1, 4, 7, 6, 9, 9, 0, 9, 0, 2, 6, 4, 0,
    1, 3, 6, 3, 9, 4, 4, 3, 7, 4, 5, 5, 3, 0, 5, 0, 6, 8, 2, 0, 3, 4,
    9, 6, 2, 5, 2, 4, 5, 1, 7, 4, 9, 3, 9, 9, 6, 5, 1, 4, 3, 1, 4, 2,
    9, 8, 0, 9, 1, 9, 0, 6, 5, 9, 2, 5, 0, 9, 3, 7, 2, 2, 1, 6, 9, 6,
    4, 6, 1, 5, 1, 5, 7, 0, 9, 8, 5, 8, 3, 8, 7, 4, 1, 0, 5, 9, 7, 8,
    8, 5, 9, 5, 9, 7, 7, 2, 9, 7, 5, 4, 9, 8, 9, 3, 0, 1, 6, 1, 7, 5,
    3, 9, 2, 8, 4, 6, 8, 1, 3, 8, 2, 6, 8, 6, 8, 3, 8, 6, 8, 9, 4, 2,
    7, 7, 4, 1, 5, 5, 9, 9, 1, 8, 5, 5, 9, 2, 5, 2, 4, 5, 9, 5, 3, 9,
    5, 9, 4, 3, 1, 0, 4, 9, 9, 7, 2, 5, 2, 4, 6, 8, 0, 8, 4, 5, 9, 8,
    7, 2, 7, 3, 6, 4, 4, 6, 9, 5, 8, 4, 8, 6, 5, 3, 8, 3, 6, 7, 3, 6,
    2, 2, 2, 6, 2, 6, 0, 9, 9, 1, 2, 4, 6, 0, 8, 0, 5, 1, 2, 4, 3, 8,
    8, 4, 3, 9, 0, 4, 5, 1, 2, 4, 4, 1, 3, 6, 5, 4, 9, 7, 6, 2, 7, 8,
    0, 7, 9, 7, 7, 1, 5, 6, 9, 1, 4, 3, 5, 9, 9, 7, 7, 0, 0, 1, 2, 9,
    6, 1, 6, 0, 8, 9, 4, 4, 1, 6, 9, 4, 8, 6, 8, 5, 5, 5, 8, 4, 8, 4,
    0, 6, 3, 5, 3, 4, 2, 2, 0, 7, 2, 2, 2, 5, 8, 2, 8, 4, 8, 8, 6, 4,
    8, 1, 5, 8, 4, 5, 6, 0, 2, 8, 5, 0, 6, 0, 1, 6, 8, 4, 2, 7, 3, 9,
    4, 5, 2, 2, 6, 7, 4, 6, 7, 6, 7, 8, 8, 9, 5, 2, 5, 2, 1, 3, 8, 5,
    2, 2, 5, 4, 9, 9, 5, 4, 6, 6, 6, 7, 2, 7, 8, 2, 3, 9, 8, 6, 4, 5,
    6, 5, 9, 6, 1, 1, 6, 3, 5, 4, 8, 8, 6, 2, 3, 0, 5, 7, 7, 4, 5, 6,
    4, 9, 8, 0, 3, 5, 5, 9, 3, 6, 3, 4, 5, 6, 8, 1, 7, 4, 3, 2, 4, 1,
    1, 2, 5, 1, 5, 0, 7, 6, 0, 6, 9, 4, 7, 9, 4, 5, 1, 0, 9, 6, 5, 9,
    6, 0, 9, 4, 0, 2, 5, 2, 2, 8, 8, 7, 9, 7, 1, 0, 8, 9, 3, 1, 4, 5,
    6, 6, 9, 1, 3, 6, 8, 6, 7, 2, 2, 8, 7, 4, 8, 9, 4, 0, 5, 6, 0, 1,
    0, 1, 5, 0, 3, 3, 0, 8, 6, 1, 7, 9, 2, 8, 6, 8, 0, 9, 2, 0, 8, 7,
    4, 7, 6, 0, 9, 1, 7, 8, 2, 4, 9, 3, 8, 5, 8, 9, 0, 0, 9, 7, 1, 4,
    9, 0, 9, 6, 7, 5, 9, 8, 5, 2, 6, 1, 3, 6, 5, 5, 4, 9, 7, 8, 1, 8,
    9, 3, 1, 2, 9, 7, 8, 4, 8, 2, 1, 6, 8, 2, 9, 9, 8, 9, 4, 8, 7, 2,
    2, 6, 5, 8, 8, 0, 4, 8, 5, 7, 5, 6, 4, 0, 1, 4, 2, 7, 0, 4, 7, 7,
    5, 5, 5, 1, 3, 2, 3, 7, 9, 6, 4, 1, 4, 5, 1, 5, 2, 3, 7, 4, 6, 2,
    3, 4, 3, 6, 4, 5, 4, 2, 8, 5, 8, 4, 4, 4, 7, 9, 5, 2, 6, 5, 8, 6,
    7, 8, 2, 1, 0, 5, 1, 1, 4, 1, 3, 5, 4, 7, 3, 5, 7, 3, 9, 5, 2, 3,
    1, 1, 3, 4, 2, 7, 1, 6, 6, 1, 0, 2, 1, 3, 5, 9, 6, 9, 5, 3, 6, 2,
    3, 1, 4, 4, 2, 9, 5, 2, 4, 8, 4, 9, 3, 7, 1, 8, 7, 1, 1, 0, 1, 4,
    5, 7, 6, 5, 4, 0, 3, 5, 9, 0, 2, 7, 9, 9, 3, 4, 4, 0, 3, 7, 4, 2,
    0, 0, 7, 3, 1, 0, 5, 7, 8, 5, 3, 9, 0, 6, 2, 1, 9, 8, 3, 8, 7, 4,
    4, 7, 8, 0, 8, 4, 7, 8, 4, 8, 9, 6, 8, 3, 3, 2, 1, 4, 4, 5, 7, 1,
    3, 8, 6, 8, 7, 5, 1, 9, 4, 3, 5, 0, 6, 4, 3, 0, 2, 1, 8, 4, 5, 3,
    1, 9, 1, 0, 4, 8, 4, 8, 1, 0, 0, 5, 3, 7, 0, 6, 1, 4, 6, 8, 0, 6,
    7, 4, 9, 1, 9, 2, 7, 8, 1, 9, 1, 1, 9, 7, 9, 3, 9, 9, 5, 2, 0, 6,
    1, 4, 1, 9, 6, 6, 3, 4, 2, 8, 7, 5, 4, 4, 4, 0, 6, 4, 3, 7, 4, 5,
    1, 2, 3, 7, 1, 8, 1, 9, 2, 1, 7, 9, 9, 9, 8, 3, 9, 1, 0, 1, 5, 9,
    1, 9, 5, 6, 1, 8, 1, 4, 6, 7, 5, 1, 4, 2, 6, 9, 1, 2, 3, 9, 7, 4,
    8, 9, 4, 0, 9, 0, 7, 1, 8, 6, 4, 9, 4, 2, 3, 1, 9, 6, 1, 5, 6, 7,
    9, 4, 5, 2, 0, 8, 0, 9, 5, 1, 4, 6, 5, 5, 0, 2, 2, 5, 2, 3, 1, 6,
    0, 3, 8, 8, 1, 9, 3, 0, 1, 4, 2, 0, 9, 3, 7, 6, 2, 1, 3, 7, 8, 5,
    5, 9, 5, 6, 6, 3, 8, 9, 3, 7, 7, 8, 7, 0, 8, 3, 0, 3, 9, 0, 6, 9,
    7, 9, 2, 0, 7, 7, 3, 4, 6, 7, 2, 2, 1, 8, 2, 5, 6, 2, 5, 9, 9, 6,
    6, 1, 5, 0, 1, 4, 2, 1, 5, 0, 3, 0, 6, 8, 0, 3, 8, 4, 4, 7, 7, 3,
    4, 5, 4, 9, 2, 0, 2, 6, 0, 5, 4, 1, 4, 6, 6, 5, 9, 2, 5, 2, 0, 1,
    4, 9, 7, 4, 4, 2, 8, 5, 0, 7, 3, 2, 5, 1, 8, 6, 6, 6, 0, 0, 2, 1,
    3, 2, 4, 3, 4, 0, 8, 8, 1, 9, 0, 7, 1, 0, 4, 8, 6, 3, 3, 1, 7, 3,
    4, 6, 4, 9, 6, 5, 1, 4, 5, 3, 9, 0, 5, 7, 9, 6, 2, 6, 8, 5, 6, 1,
    0, 0, 5, 5, 0, 8, 1, 0, 6, 6, 5, 8, 7, 9, 6, 9, 9, 8, 1, 6, 3, 5,
    7, 4, 7, 3, 6, 3, 8, 4, 0, 5, 2, 5, 7, 1, 4, 5, 9, 1, 0, 2, 8, 9,
    7, 0, 6, 4, 1, 4, 0, 1, 1, 0, 9, 7, 1, 2, 0, 6, 2, 8, 0, 4, 3, 9,
    0, 3, 9, 7, 5, 9, 5, 1, 5, 6, 7, 7, 1, 5, 7, 7, 0, 0, 4, 2, 0, 3,
    3, 7, 8, 6, 9, 9, 3, 6, 0, 0, 7, 2, 3, 0, 5, 5, 8, 7, 6, 3, 1, 7,
    6, 3, 5, 9, 4, 2, 1, 8, 7, 3, 1, 2, 5, 1, 4, 7, 1, 2, 0, 5, 3, 2,
    9, 2, 8, 1, 9, 1, 8, 2, 6, 1, 8, 6, 1, 2, 5, 8, 6, 7, 3, 2, 1, 5,
    7, 9, 1, 9, 8, 4, 1, 4, 8, 4, 8, 8, 2, 9, 1, 6, 4, 4, 7, 0, 6, 0,
    9, 5, 7, 5, 2, 7, 0, 6, 9, 5, 7, 2, 2, 0, 9, 1, 7, 5, 6, 7, 1, 1,
    6, 7, 2, 2, 9, 1, 0, 9, 8, 1, 6, 9, 0, 9, 1, 5, 2, 8, 0, 1, 7, 3,
    5, 0, 6, 7, 1, 2, 7, 4, 8, 5, 8, 3, 2, 2, 2, 8, 7, 1, 8, 3, 5, 2,
    0, 9, 3, 5, 3, 9, 6, 5, 7, 2, 5, 1, 2, 1, 0, 8, 3, 5, 7, 9, 1, 5,
    1, 3, 6, 9, 8, 8, 2, 0, 9, 1, 4, 4, 4, 2, 1, 0, 0, 6, 7, 5, 1, 0,
    3, 3, 4, 6, 7, 1, 1, 0, 3, 1, 4, 1, 2, 6, 7, 1, 1, 1, 3, 6, 9, 9,
    0, 8, 6, 5, 8, 5, 1, 6, 3, 9, 8, 3, 1, 5, 0, 1, 9, 7, 0, 1, 6, 5,
    1, 5, 1, 1, 6, 8, 5, 1, 7, 1, 4, 3, 7, 6, 5, 7, 6, 1, 8, 3, 5, 1,
    5, 5, 6, 5, 0, 8, 8, 4, 9, 0, 9, 9, 8, 9, 8, 5, 9, 9, 8, 2, 3, 8,
    7, 3, 4, 5, 5, 2, 8, 3, 3, 1, 6, 3, 5, 5, 0, 7, 6, 4, 7, 9, 1, 8,
    5, 3, 5, 8, 9, 3, 2, 2, 6, 1, 8, 5, 4, 8, 9, 6, 3, 2, 1, 3, 2, 9,
    3, 3, 0, 8, 9, 8, 5, 7, 0, 6, 4, 2, 0, 4, 6, 7, 5, 2, 5, 9, 0, 7,
    0, 9, 1, 5, 4, 8, 1, 4, 1, 6, 5, 4, 9, 8, 5, 9, 4, 6, 1, 6, 3, 7,
    1, 8, 0, 2, 7, 0, 9, 8, 1, 9, 9, 4, 3, 0, 9, 9, 2, 4, 4, 8, 8, 9,
    5, 7, 5, 7, 1, 2, 8, 2, 8, 9, 0, 5, 9, 2, 3, 2, 3, 3, 2, 6, 0, 9,
    7, 2, 9, 9, 7, 1, 2, 0, 8, 4, 4, 3, 3, 5, 7, 3, 2, 6, 5, 4, 8, 9,
    3, 8, 2, 3, 9, 1, 1, 9, 3, 2, 5, 9, 7, 4, 6, 3, 6, 6, 7, 3, 0, 5,
    8, 3, 6, 0, 4, 1, 4, 2, 8, 1, 3, 8, 8, 3, 0, 3, 2, 0, 3, 8, 2, 4,
    9, 0, 3, 7, 5, 8, 9, 8, 5, 2, 4, 3, 7, 4, 4, 1, 7, 0, 2, 9, 1, 3,
    2, 7, 6, 5, 6, 1, 8, 0, 9, 3, 7, 7, 3, 4, 4, 4, 0, 3, 0, 7, 0, 7,
    4, 6, 9, 2, 1, 1, 2, 0, 1, 9, 1, 3, 0, 2, 0, 3, 3, 0, 3, 8, 0, 1,
    9, 7, 6, 2, 1, 1, 0, 1, 1, 0, 0, 4, 4, 9, 2, 9, 3, 2, 1, 5, 1, 6,
    0, 8, 4, 2, 4, 4, 4, 8, 5, 9, 6, 3, 7, 6, 6, 9, 8, 3, 8, 9, 5, 2,
    2, 8, 6, 8, 4, 7, 8, 3, 1, 2, 3, 5, 5, 2, 6, 5, 8, 2, 1, 3, 1, 4,
    4, 9, 5, 7, 6, 8, 5, 7, 2, 6, 2, 4, 3, 3, 4, 4, 1, 8, 9, 3, 0, 3,
    9, 6, 8, 6, 4, 2, 6, 2, 4, 3, 4, 1, 0, 7, 7, 3, 2, 2, 6, 9, 7, 8,
    0, 2, 8, 0, 7, 3, 1, 8, 9, 1, 5, 4, 4, 1, 1, 0, 1, 0, 4, 4, 6, 8,
    2, 3, 2, 5, 2, 7, 1, 6, 2, 0, 1, 0, 5, 2, 6, 5, 2, 2, 7, 2, 1, 1,
    1, 6, 6, 0, 3, 9, 6, 6, 6, 5, 5, 7, 3, 0, 9, 2, 5, 4, 7, 1, 1, 0,
    5, 5, 7, 8, 5, 3, 7, 6, 3, 4, 6, 6, 8, 2, 0, 6, 5, 3, 1, 0, 9, 8,
    9, 6, 5, 2, 6, 9, 1, 8, 6, 2, 0, 5, 6, 4, 7, 6, 9, 3, 1, 2, 5, 7,
    0, 5, 8, 6, 3, 5, 6, 6, 2, 0, 1, 8, 5, 5, 8, 1, 0, 0, 7, 2, 9, 3,
    6, 0, 6, 5, 9, 8, 7, 6, 4, 8, 6, 1, 1, 7, 9, 1, 0, 4, 5, 3, 3, 4,
    8, 8, 5, 0, 3, 4, 6, 1, 1, 3, 6, 5, 7, 6, 8, 6, 7, 5, 3, 2, 4, 9,
    4, 4, 1, 6, 6, 8, 0, 3, 9, 6, 2, 6, 5, 7, 9, 7, 8, 7, 7, 1, 8, 5,
    5, 6, 0, 8, 4, 5, 5, 2, 9, 6, 5, 4, 1, 2, 6, 6, 5, 4, 0, 8, 5, 3,
    0, 6, 1, 4, 3, 4, 4, 4, 3, 1, 8, 5, 8, 6, 7, 6, 9, 7, 5, 1, 4, 5,
    6, 6, 1, 4, 0, 6, 8, 0, 0, 7, 0, 0, 2, 3, 7, 8, 7, 7, 6, 5, 9, 1,
    3, 4, 4, 0, 1, 7, 1, 2, 7, 4, 9, 4, 7, 0, 4, 2, 0, 5, 6, 2, 2, 3,
    0, 5, 3, 8, 9, 9, 4, 5, 6, 1, 3, 1, 4, 0, 7, 1, 1, 2, 7, 0, 0, 0,
    4, 0, 7, 8, 5, 4, 7, 3, 3, 2, 6, 9, 9, 3, 9, 0, 8, 1, 4, 5, 4, 6,
    6, 4, 6, 4, 5, 8, 8, 0, 7, 9, 7, 2, 7, 0, 8, 2, 6, 6, 8, 3, 0, 6,
    3, 4, 3, 2, 8, 5, 8, 7, 8, 5, 6, 9, 8, 3, 0, 5, 2, 3, 5, 8, 0, 8,
    9, 3, 3, 0, 6, 5, 7, 5, 7, 4, 0, 6, 7, 9, 5, 4, 5, 7, 1, 6, 3, 7,
    7, 5, 2, 5, 4, 2, 0, 2, 1, 1, 4, 9, 5, 5, 7, 6, 1, 5, 8, 1, 4, 0,
    0, 2, 5, 0, 1, 2, 6, 2, 2, 8, 5, 9, 4, 1, 3, 0, 2, 1, 6, 4, 7, 1,
    5, 5, 0, 9, 7, 9, 2, 5, 9, 2, 3, 0, 9, 9, 0, 7, 9, 6, 5, 4, 7, 3,
    7, 6, 1, 2, 5, 5, 1, 7, 6, 5, 6, 7, 5, 1, 3, 5, 7, 5, 1, 7, 8, 2,
    9, 6, 6, 6, 4, 5, 4, 7, 7, 9, 1, 7, 4, 5, 0, 1, 1, 2, 9, 9, 6, 1,
    4, 8, 9, 0, 3, 0, 4, 6, 3, 9, 9, 4, 7, 1, 3, 2, 9, 6, 2, 1, 0, 7,
    3, 4, 0, 4, 3, 7, 5, 1, 8, 9, 5, 7, 3, 5, 9, 6, 1, 4, 5, 8, 9, 0,
    1, 9, 3, 8, 9, 7, 1, 3, 1, 1, 1, 7, 9, 0, 4, 2, 9, 7, 8, 2, 8, 5,
    6, 4, 7, 5, 0, 3, 2, 0, 3, 1, 9, 8, 6, 9, 1, 5, 1, 4, 0, 2, 8, 7,
    0, 8, 0, 8, 5, 9, 9, 0, 4, 8, 0, 1, 0, 9, 4, 1, 2, 1, 4, 7, 2, 2,
    1, 3, 1, 7, 9, 4, 7, 6, 4, 7, 7, 7, 2, 6, 2, 2, 4, 1, 4, 2, 5, 4,
    8, 5, 4, 5, 4, 0, 3, 3, 2, 1, 5, 7, 1, 8, 5, 3, 0, 6, 1, 4, 2, 2,
    8, 8, 1, 3, 7, 5, 8, 5, 0, 4, 3, 0, 6, 3, 3, 2, 1, 7, 5, 1, 8, 2,
    9, 7, 9, 8, 6, 6, 2, 2, 3, 7, 1, 7, 2, 1, 5, 9, 1, 6, 0, 7, 7, 1,
    6, 6, 9, 2, 5, 4, 7, 4, 8, 7, 3, 8, 9, 8, 6, 6, 5, 4, 9, 4, 9, 4,
    5, 0, 1, 1, 4, 6, 5, 4, 0, 6, 2, 8, 4, 3, 3, 6, 6, 3, 9, 3, 7, 9,
    0, 0, 3, 9, 7, 6, 9, 2, 6, 5, 6, 7, 2, 1, 4, 6, 3, 8, 5, 3, 0, 6,
    7, 3, 6, 0, 9, 6, 5, 7, 1, 2, 0, 9, 1, 8, 0, 7, 6, 3, 8, 3, 2, 7,
    1, 6, 6, 4, 1, 6, 2, 7, 4, 8, 8, 8, 8, 0, 0, 7, 8, 6, 9, 2, 5, 6,
    0, 2, 9, 0, 2, 2, 8, 4, 7, 2, 1, 0, 4, 0, 3, 1, 7, 2, 1, 1, 8, 6,
    0, 8, 2, 0, 4, 1, 9, 0, 0, 0, 4, 2, 2, 9, 6, 6, 1, 7, 1, 1, 9, 6,
    3, 7, 7, 9, 2, 1, 3, 3, 7, 5, 7, 5, 1, 1, 4, 9, 5, 9, 5, 0, 1, 5,
    6, 6, 0, 4, 9, 6, 3, 1, 8, 6, 2, 9, 4, 7, 2, 6, 5, 4, 7, 3, 6, 4,
    2, 5, 2, 3, 0, 8, 1, 7, 7, 0, 3, 6, 7, 5, 1, 5, 9, 0, 6, 7, 3, 5,
    0, 2, 3, 5, 0, 7, 2, 8, 3, 5, 4, 0, 5, 6, 7, 0, 4, 0, 3, 8, 6, 7,
    4, 3, 5, 1, 3, 6, 2, 2, 2, 2, 4, 7, 7, 1, 5, 8, 9, 1, 5, 0, 4, 9,
    5, 3, 0, 9, 8, 4, 4, 4, 8, 9, 3, 3, 3, 0, 9, 6, 3, 4, 0, 8, 7, 8,
    0, 7, 6, 9, 3, 2, 5, 9, 9, 3, 9, 7, 8, 0, 5, 4, 1, 9, 3, 4, 1, 4,
    4, 7, 3, 7, 7, 4, 4, 1, 8, 4, 2, 6, 3, 1, 2, 9, 8, 6, 0, 8, 0, 9,
    9, 8, 8, 8, 6, 8, 7, 4, 1, 3, 2, 6, 0, 4, 7, 2 } ;

  const size_t nacc1 = 3 ;
  const double numacc1[3] = { 10000001, 10000003, 10000002 } ;

  const size_t nacc2 = 1001 ;
  double numacc2[1001] ;

  const size_t nacc3 = 1001 ;
  double numacc3[1001] ;

  const size_t nacc4 = 1001 ;
  double numacc4[1001] ;

  numacc2[0] = 1.2 ;
  numacc3[0] = 1000000.2 ;
  numacc4[0] = 10000000.2 ;

  for (i = 1 ; i < 1000  ; i += 2)
    {
      numacc2[i] = 1.1 ;
      numacc2[i+1] = 1.3 ;
      numacc3[i] = 1000000.1 ;
      numacc3[i+1] = 1000000.3 ;
      numacc4[i] = 10000000.1 ;
      numacc4[i+1] = 10000000.3 ;
    }

  gsl_ieee_env_setup ();

  {
    double mean = gsl_stats_mean (lew, 1, nlew);
    double sd = gsl_stats_sd (lew, 1, nlew);
    double lag1 = gsl_stats_lag1_autocorrelation (lew, 1, nlew);

    double expected_mean = -177.435000000000;
    double expected_sd = 277.332168044316;
    double expected_lag1 = -0.307304800605679;

    gsl_test_rel (mean, expected_mean, 1e-15, "lew gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-15, "lew gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-14, "lew autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (lottery, 1, nlottery);
    double sd = gsl_stats_sd (lottery, 1, nlottery);
    double lag1 = gsl_stats_lag1_autocorrelation (lottery, 1, nlottery);

    double expected_mean = 518.958715596330;
    double expected_sd = 291.699727470969;
    double expected_lag1 = -0.120948622967393;

    gsl_test_rel (mean, expected_mean, 1e-15, "lottery gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-15, "lottery gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-14, "lottery autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (mavro, 1, nmavro);
    double sd = gsl_stats_sd (mavro, 1, nmavro);
    double lag1 = gsl_stats_lag1_autocorrelation (mavro, 1, nmavro);

    double expected_mean = 2.00185600000000;
    double expected_sd = 0.000429123454003053;
    double expected_lag1 = 0.937989183438248;

    gsl_test_rel (mean, expected_mean, 1e-15, "mavro gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-13, "mavro gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-13, "mavro autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (michelson, 1, nmichelson);
    double sd = gsl_stats_sd (michelson, 1, nmichelson);
    double lag1 = gsl_stats_lag1_autocorrelation (michelson, 1, nmichelson);

    double expected_mean = 299.852400000000;
    double expected_sd = 0.0790105478190518;
    double expected_lag1 = 0.535199668621283;

    gsl_test_rel (mean, expected_mean, 1e-15, "michelson gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-13, "michelson gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-13, "michelson autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (pidigits, 1, npidigits);
    double sd = gsl_stats_sd (pidigits, 1, npidigits);
    double lag1 = gsl_stats_lag1_autocorrelation (pidigits, 1, npidigits);

    double expected_mean = 4.53480000000000;
    double expected_sd = 2.86733906028871;
    double expected_lag1 = -0.00355099287237972;

    gsl_test_rel (mean, expected_mean, 1e-14, "pidigits gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-15, "pidigits gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-14, "pidigits autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (numacc1, 1, nacc1);
    double sd = gsl_stats_sd (numacc1, 1, nacc1);
    double lag1 = gsl_stats_lag1_autocorrelation (numacc1, 1, nacc1);

    double expected_mean = 10000002;
    double expected_sd = 1;
    double expected_lag1 = -0.5;

    gsl_test_rel (mean, expected_mean, 1e-15, "acc1 gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-15, "acc1 gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-15, "acc1 autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (numacc2, 1, nacc2);
    double sd = gsl_stats_sd (numacc2, 1, nacc2);
    double lag1 = gsl_stats_lag1_autocorrelation (numacc2, 1, nacc2);

    double expected_mean = 1.2;
    double expected_sd = 0.1;
    double expected_lag1 = -0.999;

    gsl_test_rel (mean, expected_mean, 1e-15, "acc2 gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-15, "acc2 gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-10, "acc2 autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (numacc3, 1, nacc3);
    double sd = gsl_stats_sd (numacc3, 1, nacc3);
    double lag1 = gsl_stats_lag1_autocorrelation (numacc3, 1, nacc3);

    double expected_mean = 1000000.2;
    double expected_sd = 0.1;
    double expected_lag1 = -0.999;

    gsl_test_rel (mean, expected_mean, 1e-15, "acc3 gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-9, "acc3 gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-10, "acc3 autocorrelation") ;
  }

  {
    double mean = gsl_stats_mean (numacc4, 1, nacc4);
    double sd = gsl_stats_sd (numacc4, 1, nacc4);
    double lag1 = gsl_stats_lag1_autocorrelation (numacc4, 1, nacc4);

    double expected_mean = 10000000.2;
    double expected_sd = 0.1;
    double expected_lag1 = -0.999;

    gsl_test_rel (mean, expected_mean, 1e-15, "acc4 gsl_stats_mean") ;
    gsl_test_rel (sd, expected_sd, 1e-7, "acc4 gsl_stats_sd") ;
    gsl_test_rel (lag1, expected_lag1, 1e-10, "acc4 autocorrelation") ;
  }

  return 0;
}


/* specfunc/log.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_log.h"

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* Chebyshev expansion for log(1 + x(t))/x(t)
 *
 * x(t) = (4t-1)/(2(4-t))
 * t(x) = (8x+1)/(2(x+2))
 * -1/2 < x < 1/2
 * -1 < t < 1
 */
static double lopx_data[21] = {
  2.16647910664395270521272590407,
 -0.28565398551049742084877469679,
  0.01517767255690553732382488171,
 -0.00200215904941415466274422081,
  0.00019211375164056698287947962,
 -0.00002553258886105542567601400,
  2.9004512660400621301999384544e-06,
 -3.8873813517057343800270917900e-07,
  4.7743678729400456026672697926e-08,
 -6.4501969776090319441714445454e-09,
  8.2751976628812389601561347296e-10,
 -1.1260499376492049411710290413e-10,
  1.4844576692270934446023686322e-11,
 -2.0328515972462118942821556033e-12,
  2.7291231220549214896095654769e-13,
 -3.7581977830387938294437434651e-14,
  5.1107345870861673561462339876e-15,
 -7.0722150011433276578323272272e-16,
  9.7089758328248469219003866867e-17,
 -1.3492637457521938883731579510e-17,
  1.8657327910677296608121390705e-18
};
static cheb_series lopx_cs = {
  lopx_data,
  20,
  -1, 1,
  10
};

/* Chebyshev expansion for (log(1 + x(t)) - x(t))/x(t)^2
 *
 * x(t) = (4t-1)/(2(4-t))
 * t(x) = (8x+1)/(2(x+2))
 * -1/2 < x < 1/2
 * -1 < t < 1
 */
static double lopxmx_data[20] = {
 -1.12100231323744103373737274541,
  0.19553462773379386241549597019,
 -0.01467470453808083971825344956,
  0.00166678250474365477643629067,
 -0.00018543356147700369785746902,
  0.00002280154021771635036301071,
 -2.8031253116633521699214134172e-06,
  3.5936568872522162983669541401e-07,
 -4.6241857041062060284381167925e-08,
  6.0822637459403991012451054971e-09,
 -8.0339824424815790302621320732e-10,
  1.0751718277499375044851551587e-10,
 -1.4445310914224613448759230882e-11,
  1.9573912180610336168921438426e-12,
 -2.6614436796793061741564104510e-13,
  3.6402634315269586532158344584e-14,
 -4.9937495922755006545809120531e-15,
  6.8802890218846809524646902703e-16,
 -9.5034129794804273611403251480e-17,
  1.3170135013050997157326965813e-17
};
static cheb_series lopxmx_cs = {
  lopxmx_data,
  19,
  -1, 1,
  9
};

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

#ifndef HIDE_INLINE_STATIC
int
gsl_sf_log_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else {
    result->val = log(x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_log_abs_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x == 0.0) {
    DOMAIN_ERROR(result);
  }
  else {
    result->val = log(fabs(x));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}
#endif

int
gsl_sf_complex_log_e(const double zr, const double zi, gsl_sf_result * lnr, gsl_sf_result * theta)
{
  /* CHECK_POINTER(lnr) */
  /* CHECK_POINTER(theta) */

  if(zr != 0.0 || zi != 0.0) {
    const double ax = fabs(zr);
    const double ay = fabs(zi);
    const double min = GSL_MIN(ax, ay);
    const double max = GSL_MAX(ax, ay);
    lnr->val = log(max) + 0.5 * log(1.0 + (min/max)*(min/max));
    lnr->err = 2.0 * GSL_DBL_EPSILON * fabs(lnr->val);
    theta->val = atan2(zi, zr);
    theta->err = GSL_DBL_EPSILON * fabs(lnr->val);
    return GSL_SUCCESS;
  }
  else {
    DOMAIN_ERROR_2(lnr, theta);
  }
}

int
gsl_sf_log_1plusx_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0) {
    DOMAIN_ERROR(result);
  }
  else if(fabs(x) < GSL_ROOT6_DBL_EPSILON) {
    const double c1 = -0.5;
    const double c2 =  1.0/3.0;
    const double c3 = -1.0/4.0;
    const double c4 =  1.0/5.0;
    const double c5 = -1.0/6.0;
    const double c6 =  1.0/7.0;
    const double c7 = -1.0/8.0;
    const double c8 =  1.0/9.0;
    const double c9 = -1.0/10.0;
    const double t  =  c5 + x*(c6 + x*(c7 + x*(c8 + x*c9)));
    result->val = x * (1.0 + x*(c1 + x*(c2 + x*(c3 + x*(c4 + x*t)))));
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(fabs(x) < 0.5) {
    double t = 0.5*(8.0*x + 1.0)/(x+2.0);
    gsl_sf_result c;
    cheb_eval_e(&lopx_cs, t, &c);
    result->val = x * c.val;
    result->err = fabs(x * c.err);
    return GSL_SUCCESS;
  }
  else {
    result->val = log(1.0 + x);
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_log_1plusx_mx_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0) {
    DOMAIN_ERROR(result);
  }
  else if(fabs(x) < GSL_ROOT5_DBL_EPSILON) {
    const double c1 = -0.5;
    const double c2 =  1.0/3.0;
    const double c3 = -1.0/4.0;
    const double c4 =  1.0/5.0;
    const double c5 = -1.0/6.0;
    const double c6 =  1.0/7.0;
    const double c7 = -1.0/8.0;
    const double c8 =  1.0/9.0;
    const double c9 = -1.0/10.0;
    const double t  =  c5 + x*(c6 + x*(c7 + x*(c8 + x*c9)));
    result->val = x*x * (c1 + x*(c2 + x*(c3 + x*(c4 + x*t))));
    result->err = GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(fabs(x) < 0.5) {
    double t = 0.5*(8.0*x + 1.0)/(x+2.0);
    gsl_sf_result c;
    cheb_eval_e(&lopxmx_cs, t, &c);
    result->val = x*x * c.val;
    result->err = x*x * c.err;
    return GSL_SUCCESS;
  }
  else {
    const double lterm = log(1.0 + x);
    result->val = lterm - x;
    result->err = GSL_DBL_EPSILON * (fabs(lterm) + fabs(x));
    return GSL_SUCCESS;
  }
}

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

#include "eval.h"

double gsl_sf_log(const double x)
{
  EVAL_RESULT(gsl_sf_log_e(x, &result));
}

double gsl_sf_log_abs(const double x)
{
  EVAL_RESULT(gsl_sf_log_abs_e(x, &result));
}

double gsl_sf_log_1plusx(const double x)
{
  EVAL_RESULT(gsl_sf_log_1plusx_e(x, &result));
}

double gsl_sf_log_1plusx_mx(const double x)
{
  EVAL_RESULT(gsl_sf_log_1plusx_mx_e(x, &result));
}


/*
 *  test.c
 *
 *
 *  Created by ashleyr on 30/06/2010.
 *  Copyright 2010 __MyCompanyName__. All rights reserved.
 *
 */
#include "headera.h"
#include <stdlib.h>
//#include <malloc.h>
//#include <fstream>
//#include <iomanip>
#include <stdio.h>
#include <math.h>
//#include <string>
#include <time.h>
#include <gsl/gsl_math.h>
//#include <gsl/gsl_complex.h>
//#include <gsl/gsl_complex_math.h>
//#include <gsl/gsl_const_num.h>
//#include <gsl/gsl_const_mksa.h>
//#include <gsl/gsl_sf.h>
#include <gsl/gsl_linalg.h>
//#include <gsl/gsl_eigen.h>
//#include <gsl/gsl_blas.h>
//#include <gsl/gsl_permutation.h>
//#include <gsl/gsl_errno.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_integration.h>
//#include <gsl/gsl_monte.h>
//#include <gsl/gsl_monte_plain.h>
//#include <gsl/gsl_monte_miser.h>
//#include <gsl/gsl_monte_vegas.h>
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
//#include <gsl/gsl_odeiv.h>

main(int argc, char *argv[]) {
	int nran = 100000;
	int Nline_max =1000;
	double a;
	double *lcd = dvector(1,1000000);
	double *lcr = dvector(1,1000000);
	double *lsd = dvector(1,1000000);
	double *lsr = dvector(1,1000000);
	double *lzw = dvector(1,1000000);
	double *lpc = dvector(1,1000000);
	int i = 0;
	int k = 0;
	int j = 0;
	int N = 0;
	double ti = time(NULL);
	//int nbin = 99;
	double angm;
	angm = atof(argv[2]);
	//double minang = .2*pi/180.*angm;
	double minang = 0*pi/180.;
	double binsize = 0.15*angm*pi/180.;
	double maxang = angm*12.*pi/180.;
	int nbin = (maxang-minang)/binsize;
	double binl[nbin];
    double angl[nbin];
	double binlpix[nbin];
	double binedges[nbin+2];
	//double g = minang/maxang;
	for(int nb=0;nb<nbin;nb++) {
		binl[nb] = 0;
		binlpix[nb] = 0;
	}
	for(int bn=0;bn<nbin+2;bn++) {
		double num = (bn);
		double angle = maxang - binsize*num;
		if (angle < 0) angle = 0;
		double angb = maxang-binsize*num-binsize/2.;
        binedges[bn] = cos(angle);
        if (bn<nbin) {
            angl[bn] =angb;
        }
		//printf("%lf %lf %lf\n",angle,binedges[bn],cos(angle));
	}
    char fxi_name[200];
    char s1[200];
    sprintf(s1,argv[1]);
    strcpy(fxi_name,s1);
    sprintf(fxi_name,strcat(fxi_name,"odenspczw.dat"));
    FILE *fxi;
    printf("%s\n",s1);
    fxi=fopen(fxi_name,"r");
    if (fxi == NULL) perror ("Error opening file");
    int Nline=0;
    const int bsz=240;
    char buf[bsz];

    while(fgets(buf,bsz,fxi)!= NULL) {
        double cd,cr,sr,sd,zw,pc;
        //if(sscanf(buf,"%*d %f %f %f %f",&cd,&cr,&sd,&sd)!=4)
        //	err_handler("xi input");
        //puts(buf);
        sscanf(buf,"%lf %lf %lf %lf %lf %lf",&sr,&cr,&sd,&cd,&zw,&pc);
        //cd = 1.;
        //cr = 1.;
        //sr = 1.;
        //sd = 1.;
        lcd[Nline] = cd;
        lcr[Nline] = cr;
        lsr[Nline] = sr;
        lsd[Nline] = sd;
        lzw[Nline] = zw;
        lpc[Nline] = pc;
        Nline++;
    }
    printf("%i \n",Nline);
    /*	while (i<nran) {

     l[i] = rand();
     i = i+1;

     }
     */
    float be;
    float r;
    while (k<Nline)  {
        j = k+1;
        while (j<Nline)  {
            a = lcd[k]*lcd[j]*(lcr[k]*lcr[j] + lsr[k]*lsr[j]) + lsd[k]*lsd[j];
            //r = lsr[k];
            be = binedges[0];
            int ba = -1;
            while (a > be) {
                ba++;
                be = binedges[ba+1];
            }
            if (ba > -1 && ba < nbin){
                binlpix[ba] += lpc[k]*lpc[j];
                binl[ba] += lzw[k]*lzw[j]*lpc[k]*lpc[j];
            }
            j++;
            N+=1;

        }
        //printf("%f %f\n",be,r);
        k++;
    }
    double tf = time(NULL)-ti;
    printf("took %g\n",tf);
    char foname[200];
    //char sf1[200];
    strcat(s1,"2ptPixclb.dat");

    sprintf(foname,s1);
    FILE *fo;
    fo =fopen(foname,"w");
    for (int ip=0; ip<nbin; ip++) {
        fprintf(fo,"%g %g \n",angl[ip]*180./pi,binl[ip]/binlpix[ip]);
    }
    //fclose(fxi);
    //exit(0);
}