/**
 * @file iftCommon.h
 * @brief Common definitions and functions to remaining modules.
 */

#ifndef IFT_COMMON_H
#define IFT_COMMON_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdarg.h>
#include <assert.h>
#include <cblas.h>
#include <ctype.h>
#include <dirent.h>
#include <float.h>
#include <libgen.h>
#include <limits.h>
#include <math.h>
#include <mm_malloc.h>
#include <regex.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>

#ifdef __linux
#include <dirent.h>
#include <omp.h>
#endif

#if !defined(__APPLE__)
    #include <malloc.h>
#endif
#if defined(__linux)
#endif

#include "iftBasicDataTypes.h"
#include "iftMemory.h"
#include "iftDialog.h"
#include "iftString.h"

/**
 * Common definitions
 */

#ifndef PI
#define PI IFT_PI
#endif

typedef enum ift_axis_order {
  XYZ, XZY, YXZ, YZX, ZXY, ZYX
} iftAxisOrder;

#define IFT_RANDOM_SEED (unsigned int) 213344
#define IFT_MAXWEIGHT     4095.0
#define IFT_AXIS_X 0
#define IFT_AXIS_Y 1
#define IFT_AXIS_Z 2
#define IFT_PI      3.14159265358979323846
#define IFT_INTERIOR    0
#define IFT_EXTERIOR    1
#define IFT_BOTH        2
#define IFT_WHITE       0
#define IFT_GRAY        1
#define IFT_BLACK       2
#define IFT_NIL        -1
#define IFT_INCREASING  1
#define IFT_DECREASING  0
#define IFT_EPSILON     1E-07

#ifndef FALSE
#define FALSE 0
#endif
#ifndef TRUE
#define TRUE 1
#endif

/**
 * Common operations
 */

#ifndef iftMax
#define iftMax(x,y) (((x) > (y))?(x):(y))
#endif

#ifndef iftMin
#define iftMin(x,y) (((x) < (y))?(x):(y))
#endif

#define iftRound(x) ((x < 0)?(int)(x-0.5):(int)(x+0.5))
#define iftSign(x) ((x >= 0)?1:-1)

/** @brief Euclidean Distance between voxels or points */
#define iftPointDistance(u,v) (sqrtf((u.x-v.x)*(u.x-v.x)+(u.y-v.y)*(u.y-v.y)+(u.z-v.z)*(u.z-v.z)))

/** @brief Euclidean Distance between voxels or porints */
#define iftVoxelDistance(u,v) iftPointDistance(u,v)

/** @brief Computes the Squared Distance between two voxels or points */
#define iftSquaredVoxelDistance(u,v) ((u.x-v.x)*(u.x-v.x)+(u.y-v.y)*(u.y-v.y)+(u.z-v.z)*(u.z-v.z))

/* @brief Computes the Smooth Euclidean Distances from a pre-computed Squared Euclidean distance among 26-neighbors
 * @param squared_dist The pre-computed Squared Euclidean Distance among 26-neighbors. The details are in N. Kiryati and G. Sze kely, "Estimating shortest paths and minimal distances on digitized three-dimensional surfaces," Pattern Recognition, vol. 26, pp. 1623-1637, 1993.
 * @return The resulting Smooth Euclidean Distance
 */
#define iftSmoothEuclideanDistance(squared_dist) ((fabs(squared_dist-1)<1.0e-6)? 0.9016 : (fabs(squared_dist-2)<1.0e-6)? 1.289 : 1.615 )

/**
 * @brief Converts a Point to a Voxel
 * @author Samuel
 * @date May 24, 2016
 */
#define iftPointToVoxel(point) ((iftVoxel) {(int) (point).x, (int) (point).y, (int) (point).z})

/**
 * @brief Converts a Voxel to a Point
 * @author Samuel
 * @date May 24, 2016
 */
#define iftVoxelToPoint(v) ((iftPoint) {(v).x, (v).y, (v).z})

#define iftImageCenter(img) ((iftVoxel){((img)->xsize - 1)/2, ((img)->ysize - 1)/2, ((img)->zsize - 1)/2})

/** @brief Constant used to align memory */
#define IFT_MEMORY_ALIGNMENT 16

/**
 * @brief Init the GPU devices.
 * @warning If not called, the GPU devices will be started in the first call for a GPU function.
 * @author Peixinho
 */
void iftStartGPU();

/**
 * @brief Stop the GPU devices.
 * @author Peixinho
 */
void iftStopGPU();

/**
 * @brief Checks if the long positive number is prime or not.
 * @author Samuel Martins
 * @date Jan 17th, 2016
 * @note Based on https://en.wikipedia.org/wiki/Primality_test
 */
bool iftIsPrime(long n);

/**
 * @brief Computes a fast power for small integer exponents.
 * @param base
 * @param b
 * @return Returns base^b
 * @author Peixinho
 * @date May, 2016
 * @warning The funcion is faster upto b=10, after that the regular function pow is called.
 */
double iftFastPow(double base, unsigned b);

/**
 * @brief Prints a Float array on stdout of lentgh <n>.
 */
void      iftPrintFloatArray(float* v, int n);

/**
 * @brief Prints a Double array on stdout of lentgh <n>.
 */
void      iftPrintDoubleArray(double* v, int n);

/**
 * @brief Prints an Integer array on stdout of lentgh <n>.
 */
void      iftPrintIntArray(int* v, int n);

/**
 * @brief Returns a random integer number between low and high.
 */
int iftRandomInteger (int low, int high);

/**
 * @brief Randomly selects nelems of the set [low, high]
 */
int *iftRandomIntegers (int low, int high, int nelems);

/**
 * @brief Randomly selects a normal distributed (N(0,1)) float number
 */
float iftRandomNormalFloat(void);

/**
 * @brief Returns initial time
 */
timer *iftTic(void);

/**
 * @brief Returns final time
 */
timer *iftToc(void);

/**
 * @brief Prints the computational time from tstart to tend, as obtained by iftTic() and iftToc() functions.
 */
void iftPrintCompTime(timer *tstart, timer *tend, const char *msg, ...);

/**
 * @brief Computes the difference in ms from the initial time to the final time.
 *
 * @param tic The initial time.
 * @param toc The final time.
 *
 * @return The computed time difference in milliseconds.
 *
 * @warning The memory allocated for <tic> and <toc> is freed inside this function.
 */
float iftCompTime(timer *tic, timer *toc);

/**
 * @brief Returns a string with the formatted time: days hours mins secs ms.
 *
 * @author Samuel
 *
 * Given a time in ms, this function returns a formatted time: days hours mins secs ms.
 * The runtime in MILISECONDS can be obtained with the function iftCompTime(timer *tic, timer *toc).
 *
 * @param runtime Time in ms.
 * @return The formatted time.
 */
char *iftFormattedTime(float runtime);

/**
 * @brief Prints the runtime in the following format: days hours mins secs ms.
 *
 * @author Samuel Martins
 * @date October 23, 2015
 * @sa iftFormattedTime()
 *
 * Given a time in ms, this function prints the runtime following the format: days hours mins secs ms.
 * The runtime in MILISECONDS can be obtained with the function iftCompTime(timer *tic, timer *toc).
 *
 * @param runtime Time in ms.
 */
void iftPrintFormattedTime(float runtime);

/**
 * @brief Writes a timer to a given file, using the following format %s: %f, where %s is the
 * given information corresponding to the time and %f is the current time in milliseconds.
 *
 * @param tic is freed by the function.
 */
void iftWriteTimerToFile(const char *filename, const char *information, timer *tic);

/**
 * @brief Generates seed for rand(), used in iftRandomInteger.
 */
void iftRandomSeed(unsigned int);

/**
 * @brief Returns the factorial of a number or NIL in case of overflow
 */
long double iftFactorial(int n);

/**
 * @brief Returns the limit to avoid overflow in factorial computation
 */
int iftFactorialLimit(void);

void iftUnitNorm(float *feats, int nelems);
void iftNormalizeFeatures(float *feats, int nelems);

float iftSquaredFeatDistance(float *A, float *B, int n);
float iftFeatDistance(float *A, float *B, int n);

/**
 * @brief Computes the Manhattan Distance between two arrays of float considering the sinal, ie.,
 * wihout the module between the feature difference: dist = (b[0]-a[0]) + (b[1]-a[1]) + ...
 */
float iftSignedManhattanDistance(float *A, float *B, int n);

/**
 * @brief Computes the log(val) in the specified base.
 * @author Peixinho
 */
double iftLog(double val, double base);

/**
 * @brief Returns the Central Voxel from the Bounding Box
 * @author Samuel Martins
 * @date Mar 15, 2016
 * @ingroup Geometry
 */
iftVoxel iftCenterFromBoundingBox(iftBoundingBox bb);

/**
 * @brief Vector operations defined as macros so that iftVoxels, iftVectors, and iftPoints may be used interchangeably.
 */
/**
 * @brief Subtracts point vec1 from vec2 (it works for iftVectors/iftVoxels/iftPoints).
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorSub(vec1, vec2) {(vec1).x - (vec2).x, (vec1).y - (vec2).y, (vec1).z - (vec2).z}
/**
 * @brief Adds points vec1 and vec2 (it works for iftVectors/iftVoxels/iftPoints).
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorSum(vec1, vec2) {(vec1).x + (vec2).x, (vec1).y + (vec2).y, (vec1).z + (vec2).z}
/**
 * @brief Computes the cross product between two voxels, points, or vectors.
 *
 * @author Thiago Vallin Spina (changed the macro function's name)
 * @date Mar 04, 2016
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorCrossProd(a, b) {(a).y*(b).z - (a).z*(b).y, (a).z*(b).x - (a).x*(b).z, (a).x*(b).y - (a).y*(b).x}
/**
 * @brief Computes the inner product between two voxels, points, or vectors.
 *
 * @author Thiago Vallin Spina (changed the macro function's name)
 * @date Mar 04, 2016
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorInnerProduct(a, b) (((a).x*(b).x + (a).y*(b).y + (a).z*(b).z))

/**
 * @brief Computes the Vector Magnitude from a voxel or point
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 */
#define iftVectorMagnitude(v) (sqrtf(iftVectorInnerProduct((v),(v))))

/**
 * @brief Returns the XY angle of a 2D vector
 * @author Peixinho
 * @date April, 2016
 * @ingroup Geometry
 */
#define iftVectorXYAngle(v) ( (atan2(v.y, v.x)+IFT_PI) * (180.0/IFT_PI) )

/**
 * @brief Rounds a vector to integer coordinates.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorRound(vec1) {iftRound((vec1).x), iftRound((vec1).y), iftRound((vec1).z)}
/**
 * @brief Multiplies a vector by an scalar.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorScalarProd(vec, s) {(vec).x*(s), (vec).y*(s), (vec).z*(s)}
/**
 * @brief Adds an scalar to a vector.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
#define iftVectorScalarSum(vec, s) {(vec).x+(s), (vec).y+(s), (vec).z+(s)}
/**
 * @brief Divides a vector by an scalar.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @return The vector divided by the scalar, or the vector itself if the scalar is close to 0.0
 *
 * @warning The result must be cast as an iftVoxel, iftVector, or iftPoint.
 */
iftVector iftVectorScalarDiv(iftVector vec, double s);
/**
 * @brief Verifies if a vector has coordinates (0, 0, 0).
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 */
#define iftIsNullVector(vec) (iftAlmostZero((vec).x) && iftAlmostZero((vec).y) && iftAlmostZero((vec).z))
/**
 * @brief Verifies if three points, voxels, or vectors are collinear.
 *
 * This is achieved by testing if the cross product between the vectors P1->P2 and P1->P3 is Null (0,0,0).
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 */
#define iftCollinearPoints(P1, P2, P3) (iftIsNullVector((iftVector)iftVectorCrossProd((iftVector)iftVectorSub((P2), (P1)), (iftVector)iftVectorSub((P3), (P1)))))

/**
 * @brief Tests if two iftVoxels are equal
 *
 * @author Thiago Vallin Spina
 * @date Apr 19, 2016
 *
 * @param u1 The first voxel
 * @param u2 The second voxel
 * @return True if their coordinates are the same
 */
bool iftVoxelsAreEqual(iftVoxel u1, iftVoxel u2);

/**
 * @brief Computes the mean voxel
 *
 * @author Thiago Vallin Spina
 * @date May 1, 2016
 *
 * @param u1 The first voxel
 * @param u2 The second voxel
 * @return The mean voxel (u1 + u2) / 2
 */
iftVoxel iftMeanVoxel(iftVoxel u1, iftVoxel u2);

/**
 * @brief Tests if two iftPoints/iftVectors are equal
 *
 * @author Thiago Vallin Spina
 * @date Apr 19, 2016
 *
 * @param u1 The first point/vector
 * @param u2 The second point/vector
 * @return True if their coordinates are (almost) the same
 */
#define iftVectorsAreEqual(u1, u2) (iftAlmostZero((u1).x-(u2).x) && iftAlmostZero((u1).y-(u2).y) && \
                                      iftAlmostZero((u1).z-(u2).z))

/**
 * @brief Normalizes a vector if its norm is greater than 0.0, otherwise returns the vector itself.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 */
iftVector iftNormalizeVector(iftVector v);

/**
 * @brief Projects vector U onto vector V.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 *
 * @warning If vector V has norm close to 0.0 the function issues and iftError.
 */
iftVector iftProjectVector(iftVector U, iftVector V);

/**
 * @brief Returns the distance between P0 and the line from P1 to P2, whose size is P1P2
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 */
double iftVoxelLineDist2D(iftVoxel P0, iftVoxel P1, iftVoxel P2, double P1P2);

/**
 * @brief Given the vector from P1 to P0, returns the voxel projected on the line between P1 and P2.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 */
iftVoxel iftClosestVoxelOnLine2D(iftVoxel P0, iftVoxel P1, iftVoxel P2);

/**
 * @brief Returns the position of P0 with respect to the line from P1 to P2.
 *
 * Negative values indicate left side, 0 indicates on the line,
 * and positive values indicate right side.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 * @ingroup Geometry
 */
int iftVoxelLinePosition2D(iftVoxel P0, iftVoxel P1, iftVoxel P2);

/**
 * @brief Computes the circle's area given a radius.
 *
 * @author Thiago Vallin Spina
 * @date Mar 04, 2016
 *
 * @ingroup Geometry
 */
static inline double iftCircleArea(double r) {
    return (r*r)*IFT_PI;
}

/* @brief Gets rid of the carriage return and the line feed characteres introduced by DOS systems when reading
 * strings from ASCII files.
 *
 * @author Alexandre Xavier Falcao
 *
 * @param line The string in which all carriage return characters are removed.
 */
void iftRemoveCarriageReturn(char *line);

/**
 * @brief Evaluates the sigmoid function, with x = value.
 *
 * @param alfa Controls the decay of the function.
 * @author Renzo Phellan
 */
float iftSigmoidalValue(float value, float alfa);

void iftVerifyToken(FILE *fp, char *token, char *function);
void iftReadIntValue(FILE *fp, int *value, char *token, char *function);
void iftReadIntValues(FILE *fp, int **value, int nvalues, char *token, char *function);
void iftWriteIntValue(FILE *fp, int value, char *token);
void iftWriteIntValues(FILE *fp, int *value, int nvalues, char *token);
void iftReadFloatValue(FILE *fp, float *value, char *token, char *function);
void iftReadFloatValues(FILE *fp, float **value, int nvalues, char *token, char *function);
void iftWriteFloatValue(FILE *fp, float value, char *token);
void iftWriteFloatValues(FILE *fp, float *value, int nvalues, char *token);
void iftReadDoubleValue(FILE *fp, double *value, char *token, char *function);
void iftReadDoubleValues(FILE *fp, double **value, int nvalues, char *token, char *function);
void iftWriteDoubleValue(FILE *fp, double value, char *token);
void iftWriteDoubleValues(FILE *fp, double *value, int nvalues, char *token);
void iftSkipComments(FILE *fp);

/* These functions are currently used to communicate with numpy */
void iftWriteRawIntArray(char *filename, int *array, int n);
int* iftReadRawIntArray(char *filename, int n);

/**
 * @brief Computes the mean value of a float array.
 */
float iftMean(float *x, int n);
/**
 * @brief Computes the variance of a float array.
 */
float iftVar(float *x, int n);
/**
 * @brief Computes the covariance of float arrays.
 */
float iftCov(float *x, float *y, int n);

/**
 * @brief Tests if a real (float/double) number is zero. It should be used for comparisons if two floats are equal.
 *
 * @author Thiago Vallin Spina
 */
int iftAlmostZero(double x);
/**
 * @brief Computes the mod operation of <a> by <n>. If the mod value is negative, it is placed within [0,n-1].
 */
int iftSafeMod(int a, int n);

/**
 * @brief Gets an Integer Array with the Unique Elements from an Input Array.
 *
 * Ex: Given the array = [1, 2, 3, 4, 4, 3, 2, 1], with size n = 8, the resulting array of unique elements will be:
 * [1, 2, 3, 4]
 *
 * @param  array Input Int Array to be scanned.
 * @param  n     Number of Elements from the Input Int Array.
 * @return       An Integer Array with the Unique Elements from <b>array</b>.
 */
iftIntArray *iftIntArrayUnique(const int *array, int n);

/**
 * @brief Counts the Number of Unique Elements from an array.
 *
 * @param array Integer Array whose Number of Unique Elements will be counted.
 * @param n     Array Size.
 * @return      The number of Unique Elements.
 */
int iftCountUniqueIntElems(const int *array, int n);

/**
 * @brief This function finds the most suitable normalization value to be used during image color/feature conversion/normalization.
 *
 * It is afunction of the most common number of bits in imaging sensors. It assumes that these values are in {1, 8, 10,
 * 12, 16, 24, 32} bits per voxel.
 *
 * @author Alexandre Xavier Falcao.
 *
 * @param maxval The maximum value of an image, for instance.
 */
int iftNormalizationValue(int maxval);

/**
 * @brief Converts a number in radians to degrees.
 *
 * @author Thiago Vallin Spina
 */
static inline double iftDegrees(double rad) {
  return rad * 180.0 / IFT_PI;
}

/**
 * @brief Converts a number in degrees to radians.
 *
 * @author Thiago Vallin Spina
 */
static inline double iftRadians(double deg) {
  return deg * IFT_PI / 180.0;
}

/**
 * @brief Converts a negative radian number to positive.
 *
 * @author Thiago Vallin Spina
 */
static inline double iftNegativeToPositiveRadians(double rad) {
  return (rad < 0) ? 2 * IFT_PI + rad : rad;
}

/**
 * @brief Converts a negative degree number to positive.
 *
 * @author Thiago Vallin Spina
 */
static inline double iftNegativeToPositiveDegrees(double deg) {
  return (deg < 0) ? 360.0+deg : deg;
}

/**
 * @brief Compute the index of the element in vector x with minimum value
 *
 * @param x Float vector.
 * @param n Size of the vectors.
 * @return index.
*/
int iftArgmin(const int *x, int n);

/**
 * @brief Compute the index of the element in vector x with maximum value
 *
 * @param x Float vector.
 * @param n Size of the vectors.
 * @return index.
*/
int iftArgmax(const int *x, int n);

/**
 * @brief Compute the index of the element in vector x with minimum value
 *
 * @param x Float vector.
 * @param n Size of the vectors.
 * @return index.
*/
int iftFArgmin(const float *x, int n);

/**
 * @brief Compute the index of the element in vector x with maximum value
 *
 * @param x Float vector.
 * @param n Size of the vectors.
 * @return index.
*/
int iftFArgmax(const float *x, int n);

/**
 * @brief Compute the index of the element in vector x with minimum value
 *
 * @param x Double vector.
 * @param n Size of the vectors.
 * @return index.
*/
int iftDArgmin(const double *x, int n);

/**
 * @brief Compute the index of the element in vector x with maximum value
 *
 * @param x Double vector.
 * @param n Size of the vectors.
 * @return index.
*/
int iftDArgmax(const double *x, int n);

/**
 * @brief Finds the index of <elem> in array <x> and returns it.
 *
 * @author Peixinho
 *
 * @param x Unsorted array of elements.
 * @param n The size of the array.
 * @param elem The elem to be found.
 *
 * @return The element's index in <x> or NIL if not found
 */
int iftFindIntArrayElementIndex(int *x, int n, int elem);

/**
 * @brief Compute the Gaussian Probability Density Function for a set of values.
 * @author Samuel Martins
 * @date May 12, 2016
 *
 * @param  vals   Array of Values to compute the Gaussian PDF.
 * @param  n_vals Number of values.
 * @param  mean   Mean used to compute the Gaussian PDF.
 * @param  stdev  Standard Deviation used to compute the Gaussian PDF.
 * @return        Array of probabilities, one for each input value.
 */
double *iftGaussPDF(double *vals, size_t n_vals, double mean, double stdev);

/**
 * @brief Compute the Cumulative Density of the (Gaussian) PDF (Normal Distribution) in a Random Variable with value x.
 * @author Samuel Martins
 * @date May 12, 2016
 *
 * It gives the Area under the Probability Density Function (Normal Distribution) from minus infinity to x. \n
 * Then, it also describes the probability that a random variable X takes on a value less than or
 * equal to a number x. \n
 *
 * @param  x      Value from the Random Variable to compute the CDF.
 * @param  mean   Mean used to compute the Gaussian PDF.
 * @param  stdev  Standard Deviation used to compute the Gaussian PDF.
 * @return        The area under the PDF from minus to x.
 */
double iftCDF(double x, double mean, double stdev);

#ifdef __cplusplus
}
#endif

#endif


//#pragma once			/* guard */
#ifndef __UTILITIES_INCLUDED__
#define __UTILITIES_INCLUDED__
#define auxFloor(x) ((float)(long)(x))
#define auxCeil(x) ((float)(long)((x)+1))
/* fwd declarations */
/* includes */
#include <iostream>
#include <stdlib.h>
#include <sstream>
#include <fstream>
#include <math.h>
#include <vector>
#include <list>
//#include <unordered_map>
#include <map>
#include <set>
#include <algorithm>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
// declarations std lib
using namespace std;

//FUNCTION DECLARATIONS
void donowt(void);
unsigned int howmany_events(const gsl_rng *r, vector< double >& haz, double dt);
unsigned int howmany_events(const gsl_rng *r, double haz, double dt);
void gsl_ran_multinomial_vec (const gsl_rng * r,  unsigned int K,  unsigned int N, vector< double >& p, vector< unsigned int >& n);
void tridiagsolver( vector< double >& a, vector< double >& b, vector< double >& c, vector< double >& x, vector< double >& d );
void splinetable( vector< double >& X, vector< double >& Y, vector< double >& Z );
double splinefun(double x, vector< double >& X, vector< double >& Y, vector< double >& Z );
int bisect( double x, vector< double >& V, int lo, int hi);
unsigned int bisect_find (double x, vector< double >& V);
unsigned int spline_bisect_find( double x, vector< double >& V);
void find_events(const gsl_rng *r, list< unsigned int >& ES, unsigned int noeli, unsigned int noev, vector< double >& W );
int find_one_event(const gsl_rng *r, vector< double >& W );
int find_one_event(const gsl_rng *r, vector< int >& Wi );
int find_one_event(const gsl_rng *r, vector< unsigned int >& Wu );
void shuffle( const gsl_rng *r, vector< int >& w);
void MAsmooth( vector< double >& V, vector< double >& W, int L );
void MAsmooth( vector< double >& V,  int L );
int collapse_result_averager( vector< vector< double > >& ibmres, vector< vector< double > >& ibmres2 );
int collapse_result_averager( vector< vector< double > >& ibmres );

#endif


/*
Copyright 2010-2011, D. E. Shaw Research.
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.

* Neither the name of D. E. Shaw Research nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

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

/* Compute pi, using the gsl_ran_flat distribution with
   an underlying threefry4x64 counter-based rng (cbrng).
   We can call cbrng 8 times between calls to cbrng_reset */

GSL_MICRORNG(cbrng, threefry4x64); /* creates gsl_rng_cbrng */

#include "pi_check.h"

int main(int argc, char **argv){
    unsigned long hits = 0, tries = 0;
    gsl_rng *r;
    (void)argc; (void)argv; /* unused */

    threefry4x64_ctr_t c = {{0}};
    threefry4x64_key_t k = {{0}};
    r = gsl_rng_alloc(gsl_rng_cbrng);
    printf("%lu uniforms from %s\n", NTRIES, gsl_rng_name(r));
    while (tries < NTRIES) {
            double x, y;
            c.v[0]++; /* increment the counter */
            cbrng_reset(r, c, k); /* reset the rng to the new counter */
            x = gsl_ran_flat (r, -1.0, 1.0);
            y = gsl_ran_flat (r, -1.0, 1.0);
            if( x*x + y*y < 1.0 )
                hits++;
	    tries++;
    }
    gsl_rng_free (r);
    return pi_check(hits, tries);
}


/* statistics/gsl_statistics_double.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_STATISTICS_DOUBLE_H__
#define __GSL_STATISTICS_DOUBLE_H__

#include <stddef.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

GSL_EXPORT double gsl_stats_mean (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_variance (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_sd (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_variance_with_fixed_mean (const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_sd_with_fixed_mean (const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_absdev (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_skew (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_kurtosis (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_lag1_autocorrelation (const double data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_covariance (const double data1[], const size_t stride1,const double data2[], const size_t stride2, const size_t n);

GSL_EXPORT double gsl_stats_variance_m (const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_sd_m (const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_absdev_m (const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_skew_m_sd (const double data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_kurtosis_m_sd (const double data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_lag1_autocorrelation_m (const double data[], const size_t stride, const size_t n, const double mean);

GSL_EXPORT double gsl_stats_covariance_m (const double data1[], const size_t stride1,const double data2[], const size_t stride2, const size_t n, const double mean1, const double mean2);

/* DEFINED FOR FLOATING POINT TYPES ONLY */

GSL_EXPORT double gsl_stats_wmean (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_wvariance (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_wsd (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_wvariance_with_fixed_mean (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_wsd_with_fixed_mean (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_wabsdev (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_wskew (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_wkurtosis (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_wvariance_m (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double wmean);
GSL_EXPORT double gsl_stats_wsd_m (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double wmean);
GSL_EXPORT double gsl_stats_wabsdev_m (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double wmean);
GSL_EXPORT double gsl_stats_wskew_m_sd (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double wmean, const double wsd);
GSL_EXPORT double gsl_stats_wkurtosis_m_sd (const double w[], const size_t wstride, const double data[], const size_t stride, const size_t n, const double wmean, const double wsd);

/* END OF FLOATING POINT TYPES */

GSL_EXPORT double gsl_stats_pvariance (const double data1[], const size_t stride1, const size_t n1, const double data2[], const size_t stride2, const size_t n2);
GSL_EXPORT double gsl_stats_ttest (const double data1[], const size_t stride1, const size_t n1, const double data2[], const size_t stride2, const size_t n2);

GSL_EXPORT double gsl_stats_max (const double data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_min (const double data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_minmax (double * min, double * max, const double data[], const size_t stride, const size_t n);

GSL_EXPORT size_t gsl_stats_max_index (const double data[], const size_t stride, const size_t n);
GSL_EXPORT size_t gsl_stats_min_index (const double data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_minmax_index (size_t * min_index, size_t * max_index, const double data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_median_from_sorted_data (const double sorted_data[], const size_t stride, const size_t n) ;
GSL_EXPORT double gsl_stats_quantile_from_sorted_data (const double sorted_data[], const size_t stride, const size_t n, const double f) ;

__END_DECLS

#endif /* __GSL_STATISTICS_DOUBLE_H__ */


#ifndef UTIL_H
#define UTIL_H

#define PRECISION 2
#define MAX_LINE_LENGTH 9046
#define USE_CBLAS
#define USE_LAPACK

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

#ifdef USE_CBLAS
#include <cblas.h>
#endif

#ifdef USE_LAPACK
#include <lapacke.h>
#endif

/******************************************************************************
 *                                LOGGING
 ******************************************************************************/

/* DEBUG */
#ifdef NDEBUG
#define DEBUG(M, ...)
#else
#define DEBUG(M, ...) \
  fprintf(stderr, "[DEBUG] %s:%d: " M "\n", __func__, __LINE__, ##__VA_ARGS__)
#endif

/* LOG */
#define LOG_ERROR(M, ...) \
  fprintf(stderr, "[ERROR] [%s] " M "\n", __func__, ##__VA_ARGS__)
#define LOG_WARN(M, ...) fprintf(stderr, "[WARN] " M "\n", ##__VA_ARGS__)
#define LOG_INFO(M, ...) fprintf(stderr, "[INFO] " M "\n", ##__VA_ARGS__)

/* FATAL */
#define FATAL(M, ...)                                \
  fprintf(stderr, "[FATAL] " M "\n", ##__VA_ARGS__); \
  exit(-1);

/* CHECK */
#define CHECK(A, M, ...)       \
  if (!(A)) {                  \
    log_err(M, ##__VA_ARGS__); \
    goto error;                \
  }

/******************************************************************************
 *                                   DATA
 ******************************************************************************/

#if PRECISION == 1
typedef float real_t;
#elif PRECISION == 2
typedef double real_t;
#else
#error "Precision not defined!"
#endif

char *malloc_string(const char *s);
int dsv_rows(const char *fp);
int dsv_cols(const char *fp, const char delim);
char **dsv_fields(const char *fp, const char delim, int *nb_fields);
real_t **dsv_data(const char *fp, const char delim, int *nb_rows, int *nb_cols);
real_t **csv_data(const char *fp, int *nb_rows, int *nb_cols);
int **load_iarrays(const char *csv_path, int *nb_arrays);
real_t **load_darrays(const char *csv_path, int *nb_arrays);
real_t *load_vector(const char *file_path);

/******************************************************************************
 *                                 MATHS
 ******************************************************************************/

#define UNUSED(expr) \
  do {               \
    (void)(expr);    \
  } while (0)

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

#define MIN(x, y) ((x) < (y) ? (x) : (y))
#define MAX(x, y) ((x) > (y) ? (x) : (y))
#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

float randf(float a, float b);
real_t deg2rad(const real_t d);
real_t rad2deg(const real_t r);
int fltcmp(const real_t x, const real_t y);
real_t pythag(const real_t a, const real_t b);
real_t lerp(const real_t a, const real_t b, const real_t t);
void lerp3(const real_t *a, const real_t *b, const real_t t, real_t *x);
real_t sinc(const real_t x);

/******************************************************************************
 *                              LINEAR ALGEBRA
 ******************************************************************************/

void print_matrix(const char *prefix, const real_t *data, const size_t m,
                  const size_t n);
void print_vector(const char *prefix, const real_t *data, const size_t length);

void eye(real_t *A, const size_t m, const size_t n);
void ones(real_t *A, const size_t m, const size_t n);
void zeros(real_t *A, const size_t m, const size_t n);

real_t *mat_new(const size_t m, const size_t n);
int mat_cmp(const real_t *A, const real_t *B, const size_t m, const size_t n);
int mat_equals(const real_t *A, const real_t *B, const size_t m, const size_t n,
               const real_t tol);
int mat_save(const char *save_path, const real_t *A, const int m, const int n);
real_t *mat_load(const char *save_path, int *nb_rows, int *nb_cols);
void mat_set(real_t *A, const size_t stride, const size_t i, const size_t j,
             const real_t val);
real_t mat_val(const real_t *A, const size_t stride, const size_t i,
               const size_t j);
void mat_copy(const real_t *src, const int m, const int n, real_t *dest);
void mat_block_get(const real_t *A, const size_t stride, const size_t rs,
                   const size_t cs, const size_t re, const size_t ce,
                   real_t *block);
void mat_block_set(real_t *A, const size_t stride, const size_t rs,
                   const size_t cs, const size_t re, const size_t ce,
                   const real_t *block);
void mat_diag_get(const real_t *A, const int m, const int n, real_t *d);
void mat_diag_set(real_t *A, const int m, const int n, const real_t *d);
void mat_triu(const real_t *A, const size_t n, real_t *U);
void mat_tril(const real_t *A, const size_t n, real_t *L);
real_t mat_trace(const real_t *A, const size_t m, const size_t n);
void mat_transpose(const real_t *A, size_t m, size_t n, real_t *A_t);
void mat_add(const real_t *A, const real_t *B, real_t *C, size_t m, size_t n);
void mat_sub(const real_t *A, const real_t *B, real_t *C, size_t m, size_t n);
void mat_scale(real_t *A, const size_t m, const size_t n, const real_t scale);

real_t *vec_new(const size_t length);
void vec_copy(const real_t *src, const size_t length, real_t *dest);
int vec_equals(const real_t *x, const real_t *y, const size_t length);
void vec_add(const real_t *x, const real_t *y, real_t *z, size_t length);
void vec_sub(const real_t *x, const real_t *y, real_t *z, size_t length);
void vec_scale(real_t *x, const size_t length, const real_t scale);
real_t vec_norm(const real_t *x, const size_t length);

void dot(const real_t *A, const size_t A_m, const size_t A_n, const real_t *B,
         const size_t B_m, const size_t B_n, real_t *C);
void skew(const real_t x[3], real_t A[3 * 3]);
void fwdsubs(const real_t *L, const real_t *b, real_t *y, const size_t n);
void bwdsubs(const real_t *U, const real_t *y, real_t *x, const size_t n);
int check_jacobian(const char *jac_name, const real_t *fdiff, const real_t *jac,
                   const size_t m, const size_t n, const real_t tol,
                   const int print);

#ifdef USE_CBLAS
void cblas_dot(const real_t *A, const size_t A_m, const size_t A_n,
               const real_t *B, const size_t B_m, const size_t B_n, real_t *C);
#endif

/******************************************************************************
 *                                   SVD
 ******************************************************************************/

int svd(real_t *A, int m, int n, real_t *U, real_t *s, real_t *V_t);
int svdcomp(real_t *A, int m, int n, real_t *w, real_t *V);
int pinv(real_t *A, const int m, const int n, real_t *A_inv);

#ifdef USE_LAPACK

#endif

/******************************************************************************
 *                                   CHOL
 ******************************************************************************/

void chol(const real_t *A, const size_t n, real_t *L);
void chol_solve(const real_t *A, const real_t *b, real_t *x, const size_t n);

#ifdef USE_LAPACK
void lapack_chol_solve(const real_t *A, const real_t *b, real_t *x,
                       const size_t n);
#endif

/******************************************************************************
 *                                   TIME
 ******************************************************************************/

typedef uint64_t timestamp_t;

struct timespec tic();
float toc(struct timespec *tic);
float mtoc(struct timespec *tic);
float time_now();

/******************************************************************************
 *                                TRANSFORMS
 ******************************************************************************/

void tf(const real_t params[7], real_t T[4 * 4]);
void tf_params(const real_t T[4 * 4], real_t params[7]);
void tf_rot_set(real_t T[4 * 4], const real_t C[3 * 3]);
void tf_trans_set(real_t T[4 * 4], const real_t r[3]);
void tf_trans_get(const real_t T[4 * 4], real_t r[3]);
void tf_rot_get(const real_t T[4 * 4], real_t C[3 * 3]);
void tf_quat_get(const real_t T[4 * 4], real_t q[4]);
void tf_inv(const real_t T[4 * 4], real_t T_inv[4 * 4]);
void tf_point(const real_t T[4 * 4], const real_t p[3], real_t retval[3]);
void tf_hpoint(const real_t T[4 * 4], const real_t p[4], real_t retval[4]);
void tf_perturb_rot(real_t T[4 * 4], const real_t step_size, const int i);
void tf_perturb_trans(real_t T[4 * 4], const real_t step_size, const int i);
void rvec2rot(const real_t *rvec, const real_t eps, real_t *R);
void euler321(const real_t euler[3], real_t C[3 * 3]);
void rot2quat(const real_t C[3 * 3], real_t q[4]);
void quat2euler(const real_t q[4], real_t euler[3]);
void quat2rot(const real_t q[4], real_t C[3 * 3]);
void quat_lmul(const real_t p[4], const real_t q[4], real_t r[4]);
void quat_rmul(const real_t p[4], const real_t q[4], real_t r[4]);
void quat_mul(const real_t p[4], const real_t q[4], real_t r[4]);
void quat_delta(const real_t dalpha[3], real_t dq[4]);

/******************************************************************************
 *                                 IMAGE
 ******************************************************************************/

typedef struct image_t {
  int width;
  int height;
  uint8_t *data;
} image_t;

void image_init(image_t *img, uint8_t *data, int width, int height);

/******************************************************************************
 *                                  CV
 ******************************************************************************/

/******************************** RADTAN **************************************/

void radtan4_distort(const real_t params[4], const real_t p[2], real_t p_d[2]);
void radtan4_point_jacobian(const real_t params[4], const real_t p[2],
                            real_t J_point[2 * 2]);
void radtan4_params_jacobian(const real_t params[4], const real_t p[2],
                             real_t J_param[2 * 4]);

/********************************* EQUI ***************************************/

void equi4_distort(const real_t params[4], const real_t p[2], real_t p_d[2]);
void equi4_point_jacobian(const real_t params[4], const real_t p[2],
                          real_t J_point[2 * 2]);
void equi4_params_jacobian(const real_t params[4], const real_t p[2],
                           real_t J_param[2 * 4]);

/******************************** PINHOLE *************************************/

real_t pinhole_focal(const int image_width, const real_t fov);
int pinhole_project(const real_t params[4], const real_t p_C[3], real_t x[2]);

void pinhole_point_jacobian(const real_t params[4], real_t J_point[2 * 3]);
void pinhole_params_jacobian(const real_t params[4], const real_t x[2],
                             real_t J[2 * 4]);

/**************************** PINHOLE-RADTAN4 *********************************/

void pinhole_radtan4_project(const real_t params[8], const real_t p_C[3],
                             real_t x[2]);
void pinhole_radtan4_project_jacobian(const real_t params[8],
                                      const real_t p_C[3], real_t J[2 * 3]);
void pinhole_radtan4_params_jacobian(const real_t params[8],
                                     const real_t p_C[3], real_t J[2 * 8]);

/***************************** PINHOLE-EQUI4 **********************************/

void pinhole_equi4_project(const real_t params[8], const real_t p_C[3],
                           real_t x[2]);
void pinhole_equi4_project_jacobian(const real_t params[8], const real_t p_C[3],
                                    real_t J[2 * 3]);
void pinhole_equi4_params_jacobian(const real_t params[8], const real_t p_C[3],
                                   real_t J[2 * 8]);

#endif  // ZERO_H


/*
 * Copyright 2008-2016 Jan Gasthaus
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * Functions for computing and tabulating the generalized stirling numbers
 * of type (-1,-d,0).
 *
 */

#ifndef STIRLING_H_
#define STIRLING_H_
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <cassert>
#include <gsl/gsl_sf_gamma.h>

namespace gatsby { namespace libplump {

typedef std::vector<double> d_vec;
typedef std::vector<d_vec> d_vec_vec;

/**
 * computes log(exp(a) + exp(b)) while avoiding numerical
 * instabilities.
 */
inline double fast_logsumexp(double a, double b) {
  if (a>b) {
    return log(1+exp(b-a)) + a;
  } else {
    return log(1+exp(a-b)) + b;
  }
}

/**
 * computes log(exp(a) - exp(b)) while avoiding numerical
 * instabilities.
 */
inline double fast_logminusexp(double a, double b) {
  if (a>b) {
    return log(1-exp(b-a)) + a;
  } else {
    return log(1-exp(a-b)) + b;
  }
}

/**
 * Compute S_d(c,t) recursively by directly applying the recursion
 * S_d(c,t) = S_d(c-1,t-1) + (c-1 - t*d)S_d(c-1,t)
 *
 * This does a _lot_ of duplicate work and should only be used for debugging!
 */
double gen_stirling_recursive(double d, int c, int t);

/**
 * Compute S_d(c,t) recursively in log space by directly applying the recursion
 * S_d(c,t) = S_d(c-1,t-1) + (c-1 - t*d)S_d(c-1,t)
 *
 * This does a _lot_ of duplicate work and should only be used for debugging!
 */
double log_gen_stirling_recursive(double d, int c, int t);

/**
 * Compute S_d(c,t) directly in log space using the recursion
 * S_d(c,t) = S_d(c-1,t-1) + (c-1 - t*d)S_d(c-1,t)
 */
double log_gen_stirling_direct(double d, int c, int t);

double log_gen_stirling_ratio(double d, int c, int t);

d_vec_vec log_gen_stirling_table(double d, int c);

void log_gen_stirling_table_extend(double d, int c, d_vec_vec& table);

double log_get_stirling_from_table(d_vec_vec& table, int c, int t);

class stirling_generator_recompute_log {
  public:
    stirling_generator_recompute_log(double d, int c, int t);

    double ratio(int c, int t);

    static std::string statsToString();

  private:
    double d;
};

class stirling_generator_fast_log {

  public:
    stirling_generator_fast_log(double d, int c, int t);

    double get(int c, int t);

    double ratio(int c, int t);

    static std::string statsToString();

  private:
    void incC();
    void decC();

    double d;
    int current_c;
    d_vec row, col, prev_col;
};

/**
 * Class to encapsulate the generation of ratios of stirling numbers
 * for removing customers from restaurants.
 *
 * One of these objects should be constructed for each restaurant,
 * providing the current number of tables, and the total number of customers.
 */
class stirling_generator_full_log {
  public:
    stirling_generator_full_log(double d, int c, int t);

    double ratio(int c, int t);

    double getLog(int c, int t);

    static std::string statsToString();

  private:

    static int global_c_max, num_ratio_calls, num_extends, num_construct;

    d_vec_vec table;
    int c_max;
    double d;
};

inline double log_get_stirling_from_table(d_vec_vec& table, int c, int t) {
  // c and t must be non-negative
  assert(c >= 0 && t>= 0);

  if ((c == 1  && t == 1) || (c == 0 && t == 0))
    return 0;
  if (c == 0 || t == 0)
    return -INFINITY;
  if (t > c)
    return -INFINITY;
  if(c == t)
    return 0;
  if (t > (int)table.size() || (c - t) > (int)table[t-1].size()) {
    std::cout<< c << ", " << t << std::endl;
  }
  return table[t-1][c-t-1];
}

inline double log_stirling_asymptotic(double d, int c, int t) {
  return   gsl_sf_lngamma(c)
         - gsl_sf_lngamma(1 - d)
         - gsl_sf_lngamma(t)
         - (t-1) * log(d)
         - d * log(c);
}

// class log_gen_stirling_table {
//     private:
//         std::vector<std::vector<double> > table;
//         size_t c_cur, t_cur;
//
//         /*
//          * Grow the arrays to the required size.
//          */
//         void _grow(size_t c_max, size_t t_max) {
//             for(int i=t_cur;i<t_max;i++) {
//                 std::vector<double> tmp(c_cur,0);
//                 table.push_back(std::vector<double>());
//             }
//         }
//
//         /*
//          * Tabulate up to the specified limits
//          */
//         void _tabulate(size_t c_max, size_t t_max) {
//             for t in xrange(1,max_t):
//         for c in xrange(t,max_c):
//             a[c,t] = a[c-1,t-1] + (c-1-d*t)*a[c-1,t]
//
//         }
//
//     public:
//         double d;
//         gen_stirling_table(double d) : d(d), c_cur(0), t_cur(0)  {}
//
// };

}} // namespace gatsby::libplump

#endif /* STIRLING_H_ */


/**
 *
 * @file core_clarfy.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Azzam Haidar
 * @date 2011-05-15
 * @generated c Tue Jan  7 11:44:49 2014
 *
 **/
#include <math.h>
#include <lapacke.h>
#include "common.h"

#undef REAL
#define COMPLEX

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex32_t
 *
 *  CORE_clarfy applies an elementary reflector, or Householder matrix, H,
 *  to a N-by-N hermitian matrix C, from both the left and the right.
 *
 *  H is represented in the form
 *
 *     H = I - tau * v * v'
 *
 *  where  tau  is a scalar and  v  is a vector.
 *
 *  If tau is zero, then H is taken to be the unit matrix.
 *
 *******************************************************************************
 *
 * @param[in] N
 *          The number of rows and columns of the matrix C.  N >= 0.
 *
 * @param[in,out] A
 *          COMPLEX*16 array, dimension (LDA, N)
 *          On entry, the Hermetian matrix A.
 *          On exit, A is overwritten by H * A * H'.
 *
 * @param[in] LDA
 *         The leading dimension of the array A.  LDA >= max(1,N).
 *
 * @param[in] V
 *          The vector V that contains the Householder reflectors.
 *
 * @param[in] TAU
 *          The value tau.
 *
 * @param[out] WORK
 *          Workspace.
 *
 ******************************************************************************/
void
CORE_clarfy(int N,
            PLASMA_Complex32_t *A, int LDA,
            const PLASMA_Complex32_t *V,
            const PLASMA_Complex32_t *TAU,
            PLASMA_Complex32_t *WORK)
{
    static PLASMA_Complex32_t zzero =  0.0;
    static PLASMA_Complex32_t zmone = -1.0;

    int j;
    PLASMA_Complex32_t dtmp;

    /* Compute dtmp = X'*V */
    /* X = AVtau */
    cblas_chemv(CblasColMajor, CblasLower,
                N, CBLAS_SADDR(*TAU), A, LDA,
                V, 1, CBLAS_SADDR(zzero), WORK, 1);

    /* cblas_cdotc_sub(N, WORK, 1, V, 1, &dtmp);*/
    dtmp = 0.;
    for (j = 0; j < N ; j++)
        dtmp = dtmp + conjf(WORK[j]) * V[j];

    /* Compute 1/2 X'*V*t = 1/2*dtmp*tau  */
    dtmp = -dtmp * 0.5 * (*TAU);

   /* Compute W=X-1/2VX'Vt = X - dtmp*V */
    cblas_caxpy(N, CBLAS_SADDR(dtmp),
                V, 1, WORK, 1);

    /*
     * Performs the symmetric rank 2 operation
     *    A := alpha*x*y' + alpha*y*x' + A
     */
    cblas_cher2(CblasColMajor, CblasLower, N,
                CBLAS_SADDR(zmone), WORK, 1,
                                    V,    1,
                                    A,    LDA);

    return;
}
#undef COMPLEX


//This takes the output of window_univar, and applies the 1D FFT to each row or col.
//The input dim is which dim to take the 1D FFT along (0->along cols, 1->along rows).
//The output Y is power (real-valued), i.e. the FFT squared, Y = X*conj(X) element-wise.
//The size of Y must be FxC if dim==0, and RxF if dim==1, where F = nfft/2 + 1.

//I tried parallel versions with OpenMP, but much slower (have to make fftw_plan P times!).

#include <stdio.h>
#include <cblas.h>
#include <fftw3.h>

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int fft_squared_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft);
int fft_squared_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft);

int fft_squared_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft)
{
    const float z = 0.0f;
    const int F = nfft/2 + 1;
    int r, c, f;
    float *X1, *Y1;
    fftwf_plan plan;
    //struct timespec tic, toc;

    //Checks
    if (R<1) { fprintf(stderr,"error in fft_squared_s: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in fft_squared_s: C (ncols X) must be positive\n"); return 1; }
    if (nfft<R && dim==0) { fprintf(stderr,"error in fft_squared_s: nfft must be >= R (winlength)\n"); return 1; }
    if (nfft<C && dim==1) { fprintf(stderr,"error in fft_squared_s: nfft must be >= C (winlength)\n"); return 1; }

    //Initialize fftwf
    X1 = fftwf_alloc_real((size_t)nfft);
    Y1 = fftwf_alloc_real((size_t)nfft);
    plan = fftwf_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in fft_squared_s: problem creating fftw plan\n"); return 1; }
    cblas_scopy(nfft-R,&z,0,&X1[R],1); //zero-pad

    if (dim==0u)
    {
        if (iscolmajor)
        {
            //clock_gettime(CLOCK_REALTIME,&tic);
            for (c=0; c<C; c++)
            {
                cblas_scopy(R,&X[c*R],1,&X1[0],1);
                fftwf_execute(plan);

                //n = c*F; Y[n++] = fmaf(Y1[0],Y1[0],0.0f); //this was ~25% slower
                //for (f=1; f<F-1; f++, n++) { Y[n] = fmaf(Y1[f],Y1[f],fmaf(Y1[nfft-f],Y1[nfft-f],0.0f)); }
                //Y[n] = (nfft%2) ? fmaf(Y1[f],Y1[f],fmaf(Y1[nfft-f],Y1[nfft-f],0.0f)) : fmaf(Y1[f],Y1[f],0.0f);

                //n = c*F; Y[n++] = Y1[0]*Y1[0];       //this was also ~25% slower
                //for (f=1; f<F-1; f++, n++) { Y[n] = Y1[f]*Y1[f] + Y1[nfft-f]*Y1[nfft-f]; }
                //Y[n] = (nfft%2) ? Y1[f]*Y1[f] + Y1[nfft-f]*Y1[nfft-f] : Y1[f]*Y1[f];

                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[c*F],1,&Y1[0],1);
            }
            //clock_gettime(CLOCK_REALTIME,&toc);
            //fprintf(stderr,"elapsed time = %f ms\n",(toc.tv_sec-tic.tv_sec)*1e3+(toc.tv_nsec-tic.tv_nsec)/1e6);
        }
        else
        {
            for (c=0; c<C; c++)
            {
                cblas_scopy(R,&X[c],C,&X1[0],1);
                fftwf_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[c],C,&Y1[0],1);
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                cblas_scopy(C,&X[r],R,&X1[0],1);
                fftwf_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[r],R,&Y1[0],1);
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                cblas_scopy(C,&X[r*C],1,&X1[0],1);
                fftwf_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[r*F],1,&Y1[0],1);
            }
        }
    }
    else
    {
        fprintf(stderr,"error in fft_squared_s: dim must be 0 or 1.\n"); return 1;
    }

    fftwf_destroy_plan(plan); fftwf_free(X1); fftwf_free(Y1);
    return 0;
}

int fft_squared_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft)
{
    const double z = 0.0;
    const int F = nfft/2 + 1;
    int r, c, f;
    double *X1, *Y1;
    fftw_plan plan;

    //Checks
    if (R<1) { fprintf(stderr,"error in fft_squared_d: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in fft_squared_d: C (ncols X) must be positive\n"); return 1; }
    if (nfft<R && dim==0) { fprintf(stderr,"error in fft_squared_s: nfft must be >= R (winlength)\n"); return 1; }
    if (nfft<C && dim==1) { fprintf(stderr,"error in fft_squared_s: nfft must be >= C (winlength)\n"); return 1; }

    //Initialize fftw
    X1 = fftw_alloc_real((size_t)nfft);
    Y1 = fftw_alloc_real((size_t)nfft);
    plan = fftw_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in fft_squared_d: problem creating fftw plan\n"); return 1; }
    cblas_dcopy(nfft,&z,0,&X1[0],1); //zero-pad

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                cblas_dcopy(R,&X[c*R],1,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[c*F],1,&Y1[0],1);
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                cblas_dcopy(R,&X[c],C,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[c],C,&Y1[0],1);
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                cblas_dcopy(C,&X[r],R,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[r],R,&Y1[0],1);
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                cblas_dcopy(C,&X[r*C],1,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[r*F],1,&Y1[0],1);
            }
        }
    }
    else
    {
        fprintf(stderr,"error in fft_squared_d: dim must be 0 or 1.\n"); return 1;
    }

    fftw_destroy_plan(plan); fftw_free(X1); fftw_free(Y1);
    return 0;
}

#ifdef __cplusplus
}
}
#endif


//Gets deltas (1st order) and delta-deltas (2nd order) differences of X,
//which is assumed to be pre-allocated of triple size,
//Thus, for dim==0, C%3 must equal 0, and C/3 is the original ncols of X.
//And,  for dim==1, R%3 must equal 0, and R/3 is the original nrows of X.

//I implement this just like FIR for speed and shorter code,
//except non-causal and mid-sample of B is 0,
//so I don't explicitly make B (e.g., B[n] just equals sc*n).

//Note that this may treat edge samples differently than other code.
//But this could be changed with a few lines of additional code here,
//without changing the super-efficient FIR implementation.
//That is, since out-of-range samps were assumed to be 0, nothing was
//added to Y for them, so can just add something later.

#include <stdio.h>
#include <cblas.h>

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int add_delta_deltas_s (float *X, const int iscolmajor, const int R, const int C, const int dim, const int N);
int add_delta_deltas_d (double *X, const int iscolmajor, const int R, const int C, const int dim, const int N);

int add_delta_deltas_s (float *X, const int iscolmajor, const int R, const int C, const int dim, const int N)
{
    const float z = 0.0f;
    const int No = R*C/3;
    int r, c, n;
    float sc = 1.0f;

    //Checks
    if (R<1) { fprintf(stderr,"error in add_delta_deltas_s: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in add_delta_deltas_s: C (ncols X) must be positive\n"); return 1; }
    if (N<1) { fprintf(stderr,"error in add_delta_deltas_s: N (delta winlength) must be positive\n"); return 1; }
    if (dim==0 && C%3!=0) { fprintf(stderr,"error in add_delta_deltas_s: for dim==0, C (ncols X) must be divisible by 3\n"); return 1; }
    if (dim==1 && R%3!=0) { fprintf(stderr,"error in add_delta_deltas_s: for dim==1, R (nrows X) must be divisible by 3\n"); return 1; }

    //Get sc (normalizer)
    for (n=2; n<=N; n++) { sc += n*n; }
    sc = 0.5f/sc;

    if (dim==0)
    {
        if (iscolmajor)
        {
            cblas_scopy(2*No,&z,0,&X[No],1);
            for (c=0; c<C/3; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[c*R],0,&X[No+c*R],1);             //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[c*R],1,&X[No+c*R+n],1);         //past samps
                    cblas_saxpy(R-n,sc*n,&X[c*R+n],1,&X[No+c*R],1);          //future samps
                    cblas_saxpy(n,sc*n,&X[c*R+R-1],0,&X[No+c*R+R-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[No+c*R],0,&X[2*No+c*R],1);        //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[No+c*R],1,&X[2*No+c*R+n],1);    //past samps
                    cblas_saxpy(R-n,sc*n,&X[No+c*R+n],1,&X[2*No+c*R],1);     //future samps
                    cblas_saxpy(n,sc*n,&X[No+c*R+R-1],0,&X[2*No+c*R+R-n],1); //end edge samps
                }
            }
        }
        else
        {
            for (c=0; c<C/3; c++)
            {
                cblas_scopy(R,&z,0,&X[C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[c],0,&X[C/3+c],C);                      //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[c],C,&X[C/3+c+n*C],C);                //past samps
                    cblas_saxpy(R-n,sc*n,&X[c+n*C],C,&X[C/3+c],C);                 //future samps
                    cblas_saxpy(n,sc*n,&X[c+C*(R-1)],0,&X[C/3+c+C*(R-n)],C);       //end edge samps
                }
                cblas_scopy(R,&z,0,&X[2*C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[C/3+c],0,&X[2*C/3+c],C);                //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[C/3+c],C,&X[2*C/3+c+n*C],C);          //past samps
                    cblas_saxpy(R-n,sc*n,&X[C/3+c+n*C],C,&X[2*C/3+c],C);           //future samps
                    cblas_saxpy(n,sc*n,&X[C/3+c+C*(R-1)],0,&X[2*C/3+c+C*(R-n)],C); //end edge samps
                }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R/3; r++)
            {
                cblas_scopy(C,&z,0,&X[R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[r],0,&X[R/3+r],R);                      //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[r],R,&X[R/3+r+n*R],R);                //past samps
                    cblas_saxpy(C-n,sc*n,&X[r+n*R],R,&X[R/3+r],R);                 //future samps
                    cblas_saxpy(n,sc*n,&X[r+R*(C-1)],0,&X[R/3+r+R*(C-n)],R);       //end edge samps
                }
                cblas_scopy(C,&z,0,&X[2*R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[R/3+r],0,&X[2*R/3+r],R);                //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[R/3+r],R,&X[2*R/3+r+n*R],R);          //past samps
                    cblas_saxpy(C-n,sc*n,&X[R/3+r+n*R],R,&X[2*R/3+r],R);           //future samps
                    cblas_saxpy(n,sc*n,&X[R/3+r+R*(C-1)],0,&X[2*R/3+r+R*(C-n)],R); //end edge samps
                }
            }
        }
        else
        {
            cblas_scopy(2*No,&z,0,&X[No],1);
            for (r=0; r<R/3; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[r*C],0,&X[No+r*C],1);             //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[r*C],1,&X[No+r*C+n],1);         //past samps
                    cblas_saxpy(C-n,sc*n,&X[r*C+n],1,&X[No+r*C],1);          //future samps
                    cblas_saxpy(n,sc*n,&X[r*C+C-1],0,&X[No+r*C+C-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[No+r*C],0,&X[2*No+r*C],1);        //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[No+r*C],1,&X[2*No+r*C+n],1);    //past samps
                    cblas_saxpy(C-n,sc*n,&X[No+r*C+n],1,&X[2*No+r*C],1);     //future samps
                    cblas_saxpy(n,sc*n,&X[No+r*C+C-1],0,&X[2*No+r*C+C-n],1); //end edge samps
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in add_delta_deltas_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int add_delta_deltas_d (double *X, const int iscolmajor, const int R, const int C, const int dim, const int N)
{
    const double z = 0.0;
    const int No = R*C/3;
    int r, c, n;
    double sc = 1.0;

    //Checks
    if (R<1) { fprintf(stderr,"error in add_delta_deltas_d: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in add_delta_deltas_d: C (ncols X) must be positive\n"); return 1; }
    if (N<1) { fprintf(stderr,"error in add_delta_deltas_d: N (delta winlength) must be positive\n"); return 1; }
    if (dim==0 && C%3!=0) { fprintf(stderr,"error in add_delta_deltas_d: C (ncols X) must be divisible by 3\n"); return 1; }
    if (dim==1 && R%3!=0) { fprintf(stderr,"error in add_delta_deltas_d: R (nrows X) must be divisible by 3\n"); return 1; }

    //Get sc (normalizer)
    for (n=2; n<=N; n++) { sc += n*n; }
    sc = 0.5/sc;

    if (dim==0)
    {
        if (iscolmajor)
        {
            cblas_dcopy(2*No,&z,0,&X[No],1);
            for (c=0; c<C/3; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[c*R],0,&X[No+c*R],1);             //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[c*R],1,&X[No+c*R+n],1);         //past samps
                    cblas_daxpy(R-n,sc*n,&X[c*R+n],1,&X[No+c*R],1);          //future samps
                    cblas_daxpy(n,sc*n,&X[c*R+R-1],0,&X[No+c*R+R-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[No+c*R],0,&X[2*No+c*R],1);        //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[No+c*R],1,&X[2*No+c*R+n],1);    //past samps
                    cblas_daxpy(R-n,sc*n,&X[No+c*R+n],1,&X[2*No+c*R],1);     //future samps
                    cblas_daxpy(n,sc*n,&X[No+c*R+R-1],0,&X[2*No+c*R+R-n],1); //end edge samps
                }
            }
        }
        else
        {
            for (c=0; c<C/3; c++)
            {
                cblas_dcopy(R,&z,0,&X[C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[c],0,&X[C/3+c],C);                      //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[c],C,&X[C/3+c+n*C],C);                //past samps
                    cblas_daxpy(R-n,sc*n,&X[c+n*C],C,&X[C/3+c],C);                 //future samps
                    cblas_daxpy(n,sc*n,&X[c+C*(R-1)],0,&X[C/3+c+C*(R-n)],C);       //end edge samps
                }
                cblas_dcopy(R,&z,0,&X[2*C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[C/3+c],0,&X[2*C/3+c],C);                //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[C/3+c],C,&X[2*C/3+c+n*C],C);          //past samps
                    cblas_daxpy(R-n,sc*n,&X[C/3+c+n*C],C,&X[2*C/3+c],C);           //future samps
                    cblas_daxpy(n,sc*n,&X[C/3+c+C*(R-1)],0,&X[2*C/3+c+C*(R-n)],C); //end edge samps
                }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R/3; r++)
            {
                cblas_dcopy(C,&z,0,&X[R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[r],0,&X[R/3+r],R);                      //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[r],R,&X[R/3+r+n*R],R);                //past samps
                    cblas_daxpy(C-n,sc*n,&X[r+n*R],R,&X[R/3+r],R);                 //future samps
                    cblas_daxpy(n,sc*n,&X[r+R*(C-1)],0,&X[R/3+r+R*(C-n)],R);       //end edge samps
                }
                cblas_dcopy(C,&z,0,&X[2*R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[R/3+r],0,&X[2*R/3+r],R);                //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[R/3+r],R,&X[2*R/3+r+n*R],R);          //past samps
                    cblas_daxpy(C-n,sc*n,&X[R/3+r+n*R],R,&X[2*R/3+r],R);           //future samps
                    cblas_daxpy(n,sc*n,&X[R/3+r+R*(C-1)],0,&X[2*R/3+r+R*(C-n)],R); //end edge samps
                }
            }
        }
        else
        {
            cblas_dcopy(2*No,&z,0,&X[No],1);
            for (r=0; r<R/3; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[r*C],0,&X[No+r*C],1);             //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[r*C],1,&X[No+r*C+n],1);         //past samps
                    cblas_daxpy(C-n,sc*n,&X[r*C+n],1,&X[No+r*C],1);          //future samps
                    cblas_daxpy(n,sc*n,&X[r*C+C-1],0,&X[No+r*C+C-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[No+r*C],0,&X[2*No+r*C],1);        //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[No+r*C],1,&X[2*No+r*C+n],1);    //past samps
                    cblas_daxpy(C-n,sc*n,&X[No+r*C+n],1,&X[2*No+r*C],1);     //future samps
                    cblas_daxpy(n,sc*n,&X[No+r*C+C-1],0,&X[2*No+r*C+C-n],1); //end edge samps
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in add_delta_deltas_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* specfunc/mathieu_coeff.c
 *
 * Copyright (C) 2002 Lowell Johnson
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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:  L. Johnson */

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

/*****************************************************************************
 * backward_recurse
 *
 * Purpose:
 ****************************************************************************/
static void backward_recurse_c(double aa, double qq, double xx, double *ff,
                               double *gx, int even_odd, int ni)
{
  int ii, nn;
  double g1;

  g1 = *gx;
  ff[ni] = xx;

  if (even_odd == 0)
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/((4*nn*nn - aa)/qq + ff[ni-ii]);
      }
      if (ni == GSL_SF_MATHIEU_COEFF - 1)
          ff[0] *= 2.0;
  }
  else
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/(((2*nn + 1)*(2*nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }

  *gx = ff[0] - g1;
}

static void backward_recurse_s(double aa, double qq, double xx, double *ff,
                               double *gx, int even_odd, int ni)
{
  int ii, nn;
  double g1;

  g1 = *gx;
  ff[ni] = xx;

  if (even_odd == 0)
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/((4*(nn + 1)*(nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }
  else
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/(((2*nn + 1)*(2*nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }

  *gx = ff[0] - g1;
}

int gsl_sf_mathieu_a_coeff(int order, double qq, double aa, double coeff[])
{
  int ni, nn, ii, even_odd;
  double eps, g1, g2, x1, x2, e1, e2, de, xh, sum, ratio,
         ff[GSL_SF_MATHIEU_COEFF];

  eps = 1e-14;
  coeff[0] = 1.0;

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

  /* If the coefficient array is not large enough to hold all necessary
     coefficients, error out. */
  if (order > GSL_SF_MATHIEU_COEFF)
      return GSL_FAILURE;

  /* Handle the trivial case where q = 0. */
  if (qq == 0.0)
  {
      for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
          coeff[ii] = 0.0;

      coeff[order/2] = 1.0;

      return GSL_SUCCESS;
  }

  if (order < 5)
  {
      nn = 0;
      sum = 0.0;
      if (even_odd == 0)
          ratio = aa/qq;
      else
          ratio = (aa - 1 - qq)/qq;
  }
  else
  {
      if (even_odd == 0)
      {
          coeff[1] = aa/qq;
          coeff[2] = (aa - 4)/qq*coeff[1] - 2;
          sum = coeff[0] + coeff[1] + coeff[2];
          for (ii=3; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - 4*(ii - 1)*(ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += coeff[ii];
          }
      }
      else
      {
          coeff[1] = (aa - 1)/qq - 1;
          sum = coeff[0] + coeff[1];
          for (ii=2; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - (2*ii - 1)*(2*ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += coeff[ii];
          }
      }

      nn = ii - 1;

      ratio = coeff[nn]/coeff[nn-1];
  }

  ni = GSL_SF_MATHIEU_COEFF - nn - 1;

  /* Compute first two points to start root-finding. */
  if (even_odd == 0)
      x1 = -qq/(4.0*GSL_SF_MATHIEU_COEFF*GSL_SF_MATHIEU_COEFF);
  else
      x1 = -qq/((2.0*GSL_SF_MATHIEU_COEFF + 1.0)*(2.0*GSL_SF_MATHIEU_COEFF + 1.0));
  g1 = ratio;
  backward_recurse_c(aa, qq, x1, ff, &g1, even_odd, ni);
  x2 = g1;
  g2 = ratio;
  backward_recurse_c(aa, qq, x2, ff, &g2, even_odd, ni);

  /* Find the root. */
  while (1)
  {
      /* Compute the relative error. */
      e1 = g1 - x1;
      e2 = g2 - x2;
      de = e1 - e2;

      /* If we are close enough to the root, break... */
      if (fabs(de) < eps)
          break;

      /* Otherwise, determine the next guess and try again. */
      xh = (e1*x2 - e2*x1)/de;
      x1 = x2;
      g1 = g2;
      x2 = xh;
      g2 = ratio;
      backward_recurse_c(aa, qq, x2, ff, &g2, even_odd, ni);
  }

  /* Compute the rest of the coefficients. */
  sum += coeff[nn];
  for (ii=nn+1; ii<GSL_SF_MATHIEU_COEFF; ii++)
  {
      coeff[ii] = ff[ii-nn-1]*coeff[ii-1];
      sum += coeff[ii];

      /* If the coefficients are getting really small, set the remainder
         to zero. */
      if (fabs(coeff[ii]) < 1e-20)
      {
          for (; ii<GSL_SF_MATHIEU_COEFF;)
              coeff[ii++] = 0.0;
      }
  }

  /* Normalize the coefficients. */
  for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
      coeff[ii] /= sum;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_b_coeff(int order, double qq, double aa, double coeff[])
{
  int ni, nn, ii, even_odd;
  double eps, g1, g2, x1, x2, e1, e2, de, xh, sum, ratio,
         ff[GSL_SF_MATHIEU_COEFF];

  eps = 1e-10;
  coeff[0] = 1.0;

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

  /* If the coefficient array is not large enough to hold all necessary
     coefficients, error out. */
  if (order > GSL_SF_MATHIEU_COEFF)
      return GSL_FAILURE;

  /* Handle the trivial case where q = 0. */
  if (qq == 0.0)
  {
      for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
          coeff[ii] = 0.0;

      coeff[(order-1)/2] = 1.0;

      return GSL_SUCCESS;
  }

  if (order < 5)
  {
      nn = 0;
      sum = 0.0;
      if (even_odd == 0)
          ratio = (aa - 4)/qq;
      else
          ratio = (aa - 1 - qq)/qq;
  }
  else
  {
      if (even_odd == 0)
      {
          coeff[1] = (aa - 4)/qq;
          sum = 2*coeff[0] + 4*coeff[1];
          for (ii=2; ii<order/2; ii++)
          {
              coeff[ii] = (aa - 4*ii*ii)/qq*coeff[ii-1] - coeff[ii-2];
              sum += 2*(ii + 1)*coeff[ii];
          }
      }
      else
      {
          coeff[1] = (aa - 1)/qq + 1;
          sum = coeff[0] + 3*coeff[1];
          for (ii=2; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - (2*ii - 1)*(2*ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += (2*(ii + 1) - 1)*coeff[ii];
          }
      }

      nn = ii - 1;

      ratio = coeff[nn]/coeff[nn-1];
  }

  ni = GSL_SF_MATHIEU_COEFF - nn - 1;

  /* Compute first two points to start root-finding. */
  if (even_odd == 0)
      x1 = -qq/(4.0*(GSL_SF_MATHIEU_COEFF + 1.0)*(GSL_SF_MATHIEU_COEFF + 1.0));
  else
      x1 = -qq/((2.0*GSL_SF_MATHIEU_COEFF + 1.0)*(2.0*GSL_SF_MATHIEU_COEFF + 1.0));
  g1 = ratio;
  backward_recurse_s(aa, qq, x1, ff, &g1, even_odd, ni);
  x2 = g1;
  g2 = ratio;
  backward_recurse_s(aa, qq, x2, ff, &g2, even_odd, ni);

  /* Find the root. */
  while (1)
  {
      /* Compute the relative error. */
      e1 = g1 - x1;
      e2 = g2 - x2;
      de = e1 - e2;

      /* If we are close enough to the root, break... */
      if (fabs(de) < eps)
          break;

      /* Otherwise, determine the next guess and try again. */
      xh = (e1*x2 - e2*x1)/de;
      x1 = x2;
      g1 = g2;
      x2 = xh;
      g2 = ratio;
      backward_recurse_s(aa, qq, x2, ff, &g2, even_odd, ni);
  }

  /* Compute the rest of the coefficients. */
  sum += 2*(nn + 1)*coeff[nn];
  for (ii=nn+1; ii<GSL_SF_MATHIEU_COEFF; ii++)
  {
      coeff[ii] = ff[ii-nn-1]*coeff[ii-1];
      sum += 2*(ii + 1)*coeff[ii];

      /* If the coefficients are getting really small, set the remainder
         to zero. */
      if (fabs(coeff[ii]) < 1e-20)
      {
          for (; ii<GSL_SF_MATHIEU_COEFF;)
              coeff[ii++] = 0.0;
      }
  }

  /* Normalize the coefficients. */
  for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
      coeff[ii] /= sum;

  return GSL_SUCCESS;
}


#ifndef NewtonRaphsonSolver_H
#define NewtonRaphsonSolver_H

#include <stdio.h>
#include <iostream>
#include <vector>
#include <gsl/gsl_linalg.h>
#include "Node.h"
#include "ShapeBase.h"
//#include <omp.h>

/**
 *  The Newton-Raphson solver class
 *  */
class NewtonRaphsonSolver{
private:
    size_t nDim;                        ///< Dimension of the space (3D)
    size_t nNodes;                      ///< Number of nodes of the system
    double threshold;                   ///< Convergence threshold for iterations

public:
    NewtonRaphsonSolver(int nDim, int nNodes); 	///< Constructer of the N-R solver
    ~NewtonRaphsonSolver();						///< Desturctor of the N-R solver

    gsl_matrix* un;								///< The initial positions of the nodes, as calculated at the end of previous step "n"
    gsl_matrix* ge;								///< The matrix containing elastic forces on each node, size (nDim*nNodes,1). Organisation is [Node0,x ; Node0,y ; Node0,z; ... ; Noden,x ; Noden,y ; Noden,z]
    gsl_matrix* gvInternal;						///< The matrix containing internal viscous forces on each node, size (nDim*nNodes,1). Organisation is [Node0,x ; Node0,y ; Node0,z; ... ; Noden,x ; Noden,y ; Noden,z]
    gsl_matrix* gvExternal;						///< The matrix containing external viscous forces on each node, size (nDim*nNodes,1). Organisation is [Node0,x ; Node0,y ; Node0,z; ... ; Noden,x ; Noden,y ; Noden,z]
    gsl_matrix* gExt;							///< The matrix containing external forces on each node (currently includes packing forces), size (nDim*nNodes,1). Organisation is [Node0,x ; Node0,y ; Node0,z; ... ; Noden,x ; Noden,y ; Noden,z]
    gsl_vector* gSum;							///< The matrix containing sum of NewtonRaphsonSolver#ge, NewtonRaphsonSolver#gvInternal, NewtonRaphsonSolver#gvExternal, NewtonRaphsonSolver#gExt. Organisation is [Node0,x ; Node0,y ; Node0,z; ... ; Noden,x ; Noden,y ; Noden,z]
    gsl_matrix* uk;								///< The matrix storing the position of each node at iteration "k". Initiated in function NewtonRaphsonSolver#initialteUkMatrix at the beginning of each step, and updated by function NewtonRaphsonSolver#updateUkInIteration during the iteartions.
    gsl_matrix* displacementPerDt;				///< The displacement per time step of each node in current iteration "k", from its position at the end of the last time step "n"
    gsl_vector* deltaU;							///< The incremental change in positions as calculated in current iteration, resulting from the imbalance of elastic, viscous and any other external forces acting on each nodes. The solver minimises this value, convergence occurs when all incremental movements for all nodes sufficiently close to zero.
    gsl_matrix* K;								///< The Jacobian matrix, derivative of sum of forces acting on each node with respect to displacements.
    bool boundNodesWithSlaveMasterDefinition;   ///< The boolean stating if there are degrees of freedom slave to other nodes (masters).
    std::vector< std::vector<int> > slaveMasterList;    ///< The 2D integer vector storing the slave-master degrees of freedom couples, such that array [i][0] = slave to array[i][1], each i representing one dim of node position ( z of node 2 is DoF i=8);

    void setMatricesToZeroAtTheBeginningOfIteration(); 		///< The function setting the calculation matrices to zero at the beginning of each iteration.
    void setMatricesToZeroInsideIteration();												///< The function setting the relevant matrices to zero at each iteration.
    void constructUnMatrix(const std::vector<std::unique_ptr<Node> > &Nodes);               ///< This function constructs NewtonRaphsonSolver#un matrix at the beginning of the iterations.
    void initialteUkMatrix();                                                               ///< This function initiates NewtonRaphsonSolver#uk matrix at the beginning of the iterations, it is initiated to be equal to NewtonRaphsonSolver#un.
    void calculateBoundKWithSlavesMasterDoF();                                              ///< This function updates the Jacobian of the system, NewtonRaphsonSolver#K, to reflect degrees of freedom binding.
    void equateSlaveDisplacementsToMasters();                                               ///< This function moves the slaves of bound couples with a displacement equivalent to the masters'.
    void calculateDisplacementMatrix(double dt);											///< This function calculates the displacement of each node in current iteration "k", from their positions at the end of the previous step "n" (NewtonRaphsonSolver#uk - NewtonRaphsonSolver#un)
    void calcutateFixedK(const std::vector <std::unique_ptr<Node>>& Nodes);					///< This function updates the Jacobian to account for nodes  that are fixed in certain dimensions in space, as part of boundary conditions.
    void calculateForcesAndJacobianMatrixNR(const std::vector <std::unique_ptr<Node>>& Nodes, const std::vector <std::unique_ptr<ShapeBase>>&  Elements, double dt );	///< This function calculates elemental forces and Jacobians, later to be combined in NewtonRaphsonSolver#K and NewtonRaphsonSolver#gSum
    void writeForcesTogeAndgvInternal(const std::vector <std::unique_ptr<Node>>& Nodes, const std::vector <std::unique_ptr<ShapeBase>>&  Elements, std::vector<std::array<double,3>>& SystemForces);	///< This function writes the values of elemental elastic (ShapeBase#ge) and internal viscous forces (ShapeBase#gvInternal) into the system elastic and internal viscous forces, NewtonRaphsonSolver#ge, and NewtonRaphsonSolver#gvInternal, respectively.
    void writeImplicitElementalKToJacobian(const std::vector <std::unique_ptr<ShapeBase>>&  Elements);	///< This function writes the elemental values for elastic part of the Jacobian - stiffness matrix - (ShapeBase#TriPointKe) and for viscous part of Jacobian (ShapeBase#TriPointKv) into the system Jacobian NewtonRaphsonSolver#K.
    void calculateExternalViscousForcesForNR(const std::vector <std::unique_ptr<Node>>& Nodes);         ///< This function calculates the external viscous forces acting on each node, the values are sotred in NewtonRaphsonSolver#gvExternal
    void addImplicitKViscousExternalToJacobian(const std::vector <std::unique_ptr<Node>>& Nodes, double dt);	///< This function adds the external related terms of the Jacobian to the system Jacobian NewtonRaphsonSolver#K.
    void checkJacobianForAblatedNodes(std::vector <int> & AblatedNodes);                    ///< This functions checks the Jacobian to ensure the diagonal terms are non-zero for ablated nodes.
    void calculateSumOfInternalForces();                                                    ///< This function adds the ealsticity and viscosity related forces (NewtonRaphsonSolver#ge, NewtonRaphsonSolver#gvInternal, NewtonRaphsonSolver#gvExternal) to sum of forces, NewtonRaphsonSolver#gSum.
	void addExernalForces();

    void solveForDeltaU();                                                                  ///< This function solves for the displacements within the N-R step.
    //raw pointers necessary for Pardiso
	int  solveWithPardiso(double* a, double*b, int* ia, int* ja, const int n_variables);
    void constructiaForPardiso(int* ia, const int nmult, std::vector<int> &ja_vec, std::vector<double> &a_vec);
    void writeKinPardisoFormat(const int nNonzero, std::vector<int> &ja_vec, std::vector<double> &a_vec, int* ja, double* a);
	void writeginPardisoFormat(double* b, const int n);
    bool checkConvergenceViaDeltaU();                                                      ///< Check for cenvergence with the norm of displacements vector,against the threshold NewtonRaphsonSolver#threshold.
    bool checkConvergenceViaForce();                                                       ///< Check for cenvergence with the norm of forces vector,against the threshold NewtonRaphsonSolver#threshold.
    void updateUkInIteration();                                                            ///< Calulate the nodal displacemetns at the kth iteration of NR dolver.

    void displayMatrix(gsl_matrix* mat, std::string matname);
    void displayMatrix(gsl_vector* mat, std::string matname);

	bool checkIfCombinationExists(int dofSlave, int dofMaster);
    void checkMasterUpdate(int& dofMaster, int& masterId); ///< This function takes a degree of freedom number as input. This DOF is supposed to be a master. If, the dof is already a slave to another dof, then update the masetr dof. Anything that would be bound to the input dof can be bound to the already existing master of the input dof.
    bool checkIfSlaveIsAlreadyMasterOfOthers(int dofSlave, int dofMaster); ///< This function checks if the slave DOF is already master of others, if so, updates the master of said slave to the new master the current slave will be bound to.

    void updateElementPositions(const std::vector<std::unique_ptr<Node> > &Nodes, const std::vector <std::unique_ptr<ShapeBase>>&  Elements);

};

#endif


//
// Created by pedram pakseresht on 2/18/21.
//

#include <petsc.h>

int main( int argc, char *argv[] )
{

    Vec a,b,c;
    DM packer;
    PetscErrorCode ierr;
    PetscInt i[4] = {0,1,2,3};
    PetscReal v[4] = {1.5,2.5,3.5,4.5};
    PetscReal w[4] = {5.5,6.5,7.5,8.5};

    // initialize Petsc ...
    ierr = PetscInitialize(&argc, &argv, NULL, "");CHKERRQ(ierr);

    ierr= VecCreate(PETSC_COMM_WORLD, &a); CHKERRQ(ierr);
    ierr= VecCreate(PETSC_COMM_WORLD, &b); CHKERRQ(ierr);

    ierr= VecSetSizes(a,PETSC_DECIDE,4); CHKERRQ(ierr);
    ierr = VecSetFromOptions(a); CHKERRQ(ierr);
    ierr= VecSetValues(a,4,i,v,INSERT_VALUES);CHKERRQ(ierr);
    ierr= VecAssemblyBegin(a);CHKERRQ(ierr);
    ierr = VecAssemblyEnd(a);CHKERRQ(ierr);
    ierr = VecView(a,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);

    ierr= VecSetSizes(b,PETSC_DECIDE,4); CHKERRQ(ierr);
    ierr = VecSetFromOptions(b); CHKERRQ(ierr);
    ierr= VecSetValues(b,4,i,w,INSERT_VALUES);CHKERRQ(ierr);
    ierr= VecAssemblyBegin(b);CHKERRQ(ierr);
    ierr = VecAssemblyEnd(b);CHKERRQ(ierr);
    ierr = VecView(b,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);

    ierr = PetscFinalize();exit(ierr);

}


/**
 *
 * @file example_dpotrf.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @brief Example of Cholesky factorization
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:20 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <time.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_dmain.h"

static int check_factorization(int, int, double*, double*, int, int*);

int testing_dgetrf(int argc, char **argv)
{
	int M	  = 1000;
    int N     = 500;
    int LDA   = 1000;
    int info_factorization;

    double *A1   = (double *)malloc(LDA*N*sizeof(double));
    double *A2   = (double *)malloc(LDA*N*sizeof(double));

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)){
        printf("Out of Memory \n ");
        return 0;
    }

    /* Initialize A1 and A2 */
	LAPACKE_dlarnv(3,ISEED, LDA*N, A1);

	int i;
	for(i = 0; i < N*LDA; ++i){
		A2[i] = A1[i];
	}
    /* Plasma routines */
	int *IPIV = calloc(LDA, sizeof(int));
    PLASMA_dgetrf(M, N, A2, LDA, IPIV);

    /* Check the factorization */
    info_factorization = check_factorization( M, N, A1, A2, LDA, IPIV);

    if ( info_factorization != 0 )
       printf("-- Error in DGETRF example ! \n");
    else
       printf("-- Run of DGETRF example successful ! \n");

    free(A1); free(A2);

    return 0;
}

static int check_factorization(int M, int N, double *A1, double *A2, int LDA, int *IPIV)
{
    double Anorm, Rnorm;
	double Anorm1, Rnorm1;
    double alpha;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

//    double *Residual = (double *)malloc(M*N*sizeof(double));
//    double *L1       = (double *)malloc(M*N*sizeof(double));
//    double *L2       = (double *)malloc(M*N*sizeof(double));
    double *work              = (double *)malloc(N*sizeof(double));
	int *ipiv0 = calloc(LDA, sizeof(int));
	int dff = 0;

	LAPACKE_dgetrf(LAPACK_COL_MAJOR, M, N, A1, M, ipiv0);

	for(j=0; j<N; j++)
		dff += IPIV[j] - ipiv0[j];
	printf("dff: %d\n", dff);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), M, N, A1, M, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), M, N, A2, LDA, work);

	Rnorm1 = PLASMA_dlange(PlasmaInfNorm, M, N, A1, LDA);
	Anorm1 = PLASMA_dlange(PlasmaInfNorm, M, N, A2, LDA);

	printf("|Rnorm-Rnorm1|: %e, |Anorm-Anorm1|: %e\n", fabs(Rnorm-Rnorm1), fabs(Anorm-Anorm1));
    printf("============\n");
    printf("Checking the LU Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",fabs(Rnorm-Anorm)/(Anorm));

    if ( isnan(fabs(Rnorm-Anorm)/(Anorm)) || (fabs(Rnorm-Anorm)/(Anorm) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

//    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}