#include <stdio.h>
#include <stdlib.h>
#include <QuEST.h>
#include "QuEST_debug.h"
#include "QuEST_tn.h"
#include <cblas.h>

#define DEBUG 0

#if DEBUG
# define DEBUG_PRINT(x) printf x
#else
# define DEBUG_PRINT(x) do {} while (0)
#endif

int* getTensorIndexPermutation(int* contractionIndices, int numContractions,
        int* freeIndices, int numFreeIndices, int tensor){

    int *perm = (int*) malloc((numContractions+numFreeIndices)*sizeof(int));

    int firstSize, secondSize;
    int *firstArray, *secondArray;
    if (tensor==1){
        firstSize=numFreeIndices;
        firstArray=freeIndices;
        secondSize=numContractions;
        secondArray=contractionIndices;
    } else {
        secondSize=numFreeIndices;
        secondArray=freeIndices;
        firstSize=numContractions;
        firstArray=contractionIndices;
    }

    for (int i=0; i<firstSize; i++){
        perm[i]=firstArray[i];
    }
    for (int i=0; i<secondSize; i++){
        perm[i+firstSize]=secondArray[i];
    }
    return perm;
}

/*
 * All indices are of size 2
 */
int* getTensorSizes(int numQubits){
    int *sizes = (int*) malloc(numQubits*sizeof(int));
    for (int i=0; i<numQubits; i++) sizes[i]=2;
    return sizes;
}

Tensor createTensor(int numPq, int numVq, QuESTEnv env){
    Tensor tensor;

    //! Probably don't need to store these any more
    tensor.numPq = numPq;
    tensor.numVq = numVq;

    tensor.qureg = createQureg(tensor.numPq + tensor.numVq, env);
    //! might need to store next available vq

    return tensor;
}

/*
 * Swap corresponding qubit IDs in perm and return new index
 */
int swapBits(long long int b, int *perm, int nQubits) {
  long long int out = 0;
  for(int j=0;j<nQubits; j++) {
    out += (b >> j & 1) * (1 << perm[j]);
  }
  return out;
}

qreal* permuteArray(Qureg qureg, int *perm) {
  qreal* outArr = (qreal *) malloc(sizeof(qreal)*2*qureg.numAmpsPerChunk);
  for (long long int i = 0; i < qureg.numAmpsPerChunk; i++) {
    long long int newIndex = 2*swapBits(i, perm, qureg.numQubitsRepresented);
    outArr[newIndex] = qureg.stateVec.real[i];
    outArr[newIndex+1] = qureg.stateVec.imag[i];
  }
  return outArr;
}

/*
 * Expects tensor1Contractions to be in order from smallest to largest.
 * Expects tensor2Contractions to be ordered to match indices in tensor1.
 * Ie if tensor1 and tensor2 each have 3 indices and (tensor1, index 1) is
 * contracted with (tensor2, index 2) and (tensor1, index 2) is contracted with
 * (tensor2, index 0), use:
 * tensor1Contractions = [1, 2]
 * tensor2Contractions = [2, 0]
 */
Tensor contractIndices(Tensor tensor1, Tensor tensor2,
        int *tensor1Contractions, int *tensor2Contractions, int numContractions,
        int *tensor1FreeIndices, int numTensor1FreeIndices,
        int *tensor2FreeIndices, int numTensor2FreeIndices,
        QuESTEnv env){

    printf("Begin contracting\n");
    int numTensor1Qubits, numTensor2Qubits;
    numTensor1Qubits = tensor1.qureg.numQubitsRepresented;
    numTensor2Qubits = tensor2.qureg.numQubitsRepresented;
    int totalNumQ = numTensor1FreeIndices + numTensor2FreeIndices;

    printf("Permute indices to transform tensor contraction into the form of a matrix matrix multiply\n");
    int *tensor1Perm = getTensorIndexPermutation(tensor1Contractions, numContractions,
						 tensor1FreeIndices, numTensor1FreeIndices, 1);
    int *tensor2Perm = getTensorIndexPermutation(tensor2Contractions, numContractions,
						 tensor2FreeIndices, numTensor2FreeIndices, 2);

    qreal* tensor1StateVecPermuted = permuteArray(tensor1.qureg, tensor1Perm);
    qreal* tensor2StateVecPermuted = permuteArray(tensor2.qureg, tensor2Perm);

    // Working output array for BLAS routine. This needs to be separate to contractedQureg as
    // that represents complex numbers as a struct of arrays where BLAS will output an array of complex types
    qreal* outputQureg = (qreal *) malloc(sizeof(qreal)*(1LL << (totalNumQ+1LL)));

    // Dimensions of MM multiply
    int M, N, K;
    M = 1 << numTensor2FreeIndices;
    N = 1 << numTensor1FreeIndices;
    K = 1 << numContractions;
    qreal alpha[2] = {1.0, 0.0};
    qreal beta[2] = {0.0, 0.0};

    printf("MM multiply dimensions: M:%d N:%d K:%d\n", M, N, K);

    printf("Do MM multiply\n");
    // tensor2 x tensor1.
    cblas_zgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans, M, N, K, alpha,
    		tensor2StateVecPermuted, K, tensor1StateVecPermuted, N, beta, outputQureg, N);

    // Copy output into qureg object
    Qureg contractedQureg = createQureg(totalNumQ, env);
    for(long long int index = 0; index < contractedQureg.numAmpsPerChunk; index++) {
      contractedQureg.stateVec.real[index] = outputQureg[2*index];
      contractedQureg.stateVec.imag[index] = outputQureg[2*index+1];
    }

    printf("Free memory\n");
    free(tensor1StateVecPermuted);
    free(tensor2StateVecPermuted);
    free(outputQureg);

    free(tensor1Perm);
    free(tensor2Perm);

    // Free old quregs
    destroyQureg(tensor1.qureg, env);
    destroyQureg(tensor2.qureg, env);

    // Create output tensor
    Tensor outputTensor;
    outputTensor.qureg = contractedQureg;
    //! TODO: We may need to set total number of qubits here
    //outputTensor.numPq = totalNumPq;
    //outputTensor.numVq = totalNumVq;

    outputTensor.qureg = contractedQureg;
    return outputTensor;
}

// ----- operations ------------------------------------------------------------

/** Place target virtual qubit in the zero state
 * @param[in,out] tensor the tensor object
 */
void initVirtualTarget(Tensor tensor, int virtualTargetIndex){
         int vqIndex = virtualTargetIndex + tensor.numPq;
     printf("vqIndex: %d\n", vqIndex);

    Qureg qureg = tensor.qureg;

    long long int stateVecSize;
    long long int index;

    // dimension of the state vector
    // TODO: This won't work in parallel
    stateVecSize = 1LL << vqIndex;

    qreal *stateVecReal = qureg.stateVec.real;
    qreal *stateVecImag = qureg.stateVec.imag;

# ifdef _OPENMP
# pragma omp parallel \
    default  (none) \
    shared   (stateVecSize, stateVecReal, stateVecImag) \
    private  (index)
# endif
    {
# ifdef _OPENMP
# pragma omp for schedule (static)
# endif
        for (index=0; index<stateVecSize; index++) {
            stateVecReal[index+stateVecSize] = 0;
            stateVecImag[index+stateVecSize] = 0;
        }
    }
}

/** Initialize virtual qubit to |0> + |1>
 * NOTE: not (1/sqrt(2))(|0> + |1>)
 * @param[in,out] tensor the tensor object
 * @param[in] virtual qubit to initialize. Index is local to a tensor but includes all physical qubits in the tensor
 */
void initVirtualControl(Tensor tensor, int virtualControlIndex){
    int vqIndex = virtualControlIndex + tensor.numPq;
    Qureg qureg = tensor.qureg;

    long long int stateVecSize;
    long long int index;

    // dimension of the state vector
    // TODO: This won't work in parallel
    stateVecSize = 1LL << vqIndex;

    qreal *stateVecReal = qureg.stateVec.real;
    qreal *stateVecImag = qureg.stateVec.imag;

# ifdef _OPENMP
# pragma omp parallel \
    default  (none) \
    shared   (stateVecSize, stateVecReal, stateVecImag) \
    private  (index)
# endif
    {
# ifdef _OPENMP
# pragma omp for schedule (static)
# endif
        for (index=0; index<stateVecSize; index++) {
            stateVecReal[index+stateVecSize] = stateVecReal[index];
            stateVecImag[index+stateVecSize] = stateVecImag[index];
        }
    }
}


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

/*
 * This module contains routines related to the recursive median filter. The
 * algorithm is based on this paper,
 *
 * [1] S-J Ko, Y. H. Lee, and A. T. Fam, Efficient Implementation of One-Dimensional
 * Recursive Median Filters, IEEE Transactions on Circuits and Systems, Vol 37,
 * No 11, 1990.
 */

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_movstat.h>

typedef struct
{
  const gsl_movstat_accum * minmax_acc; /* minimum/maximum accumulator */
  void *minmax_state;                   /* minimum/maximum accumulator workspace */
} rmedian_state_t;

static size_t rmedian_size(const size_t n);
static int rmedian_init(const size_t n, void * vstate);
static int rmedian_insert(const double x, void * vstate);
static int rmedian_delete(void * vstate);
static int rmedian_get(void * params, double * result, const void * vstate);

static const gsl_movstat_accum rmedian_accum_type;

gsl_filter_rmedian_workspace *
gsl_filter_rmedian_alloc(const size_t K)
{
  gsl_filter_rmedian_workspace *w;
  size_t state_size;

  w = calloc(1, sizeof(gsl_filter_rmedian_workspace));
  if (w == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for workspace", GSL_ENOMEM);
    }

  w->H = K / 2;
  w->K = 2*w->H + 1;
  w->minmaxacc = gsl_movstat_accum_minmax;

  w->window = malloc(w->K * sizeof(double));
  if (w->window == NULL)
    {
      gsl_filter_rmedian_free(w);
      GSL_ERROR_NULL ("failed to allocate space for window", GSL_ENOMEM);
    }

  state_size = rmedian_size(w->H + 1);

  w->state = malloc(state_size);
  if (w->state == NULL)
    {
      gsl_filter_rmedian_free(w);
      GSL_ERROR_NULL ("failed to allocate space for min/max state", GSL_ENOMEM);
    }

  w->movstat_workspace_p = gsl_movstat_alloc_with_size(state_size, 0, w->H);
  if (!w->movstat_workspace_p)
    {
      gsl_filter_rmedian_free(w);
      GSL_ERROR_NULL ("failed to allocate space for movstat workspace", GSL_ENOMEM);
    }

  return w;
}

void
gsl_filter_rmedian_free(gsl_filter_rmedian_workspace * w)
{
  if (w->state)
    free(w->state);

  if (w->window)
    free(w->window);

  if (w->movstat_workspace_p)
    gsl_movstat_free(w->movstat_workspace_p);

  free(w);
}

/*
gsl_filter_rmedian()
  Recursive median filter

Inputs: endtype - end point handling
        x       - input vector
        y       - output vector
        w       - workspace
*/

int
gsl_filter_rmedian(const gsl_filter_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_filter_rmedian_workspace * w)
{
  if (x->size != y->size)
    {
      GSL_ERROR("input and output vectors must have same length", GSL_EBADLEN);
    }
  else
    {
      int status = GSL_SUCCESS;
      const size_t n = x->size;
      const int H = (int) w->H;
      double yprev;
      int wsize;

      /* find median of first window to initialize filter */
      wsize = gsl_movstat_fill(endtype, x, 0, H, H, w->window);
      yprev = gsl_stats_median(w->window, 1, wsize);
      gsl_vector_set(y, 0, yprev);

      if (x->size > 1)
        {
          gsl_vector_const_view xv = gsl_vector_const_subvector(x, 1, n - 1);
          gsl_vector_view yv = gsl_vector_subvector(y, 1, n - 1);

          /* apply recursive median filter to x[2:end] */
          status = gsl_movstat_apply_accum(endtype, &xv.vector, &rmedian_accum_type, (void *) &yprev, &yv.vector,
                                           NULL, w->movstat_workspace_p);
        }

      return status;
    }
}

static size_t
rmedian_size(const size_t n)
{
  size_t size = 0;
  const gsl_movstat_accum * acc = gsl_movstat_accum_minmax;

  size += sizeof(rmedian_state_t);
  size += (acc->size)(n);

  return size;
}

static int
rmedian_init(const size_t n, void * vstate)
{
  rmedian_state_t * state = (rmedian_state_t *) vstate;

  state->minmax_acc = gsl_movstat_accum_minmax;
  state->minmax_state = (void *) ((unsigned char *) vstate + sizeof(rmedian_state_t));

  (state->minmax_acc->init)(n, state->minmax_state);

  return GSL_SUCCESS;
}

static int
rmedian_insert(const double x, void * vstate)
{
  rmedian_state_t * state = (rmedian_state_t *) vstate;
  return (state->minmax_acc->insert)(x, state->minmax_state);
}

static int
rmedian_delete(void * vstate)
{
  rmedian_state_t * state = (rmedian_state_t *) vstate;
  return (state->minmax_acc->delete_oldest)(state->minmax_state);
}

static int
rmedian_get(void * params, double * result, const void * vstate)
{
  const rmedian_state_t * state = (const rmedian_state_t *) vstate;
  double *yprev = (double *) params; /* previous filter output */
  double y;                          /* new filter output */
  double xminmax[2];

  /* get minimum/maximum values of {x_i,...,x_{i+H}} */
  (state->minmax_acc->get)(NULL, xminmax, state->minmax_state);

  /* y = median [ yprev, xmin, xmax ] */
  if (*yprev <= xminmax[0])
    y = xminmax[0];
  else if (*yprev <= xminmax[1])
    y = *yprev;
  else
    y = xminmax[1];

  *result = y;
  *yprev = y;

  return GSL_SUCCESS;
}

static const gsl_movstat_accum rmedian_accum_type =
{
  rmedian_size,
  rmedian_init,
  rmedian_insert,
  rmedian_delete,
  rmedian_get
};


/*
** TFCE heuristic
**
** M. Kuhlmann, MPI-KYB, Sept 2015
*/

#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <via/via.h>

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

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

#define ABS(x) ((x) > 0 ? (x) : -(x))

VImage tfce(VImage t_image,VImage map,int nstrata)
{
  int b,r,c,i,j,k;
  size_t nslices = VImageNBands(t_image);
  size_t nrows  = VImageNRows(t_image);
  size_t ncols  = VImageNColumns(t_image);
  double tiny = 1e-16;

  /* if pixel map is not given, compute it */
  if(!map) {
    VImage mask = VCreateImage(nslices,nrows,ncols,VBitRepn);
    VFillImage(mask,VAllBands,0);

    size_t nvox = 0;
    float u;
    for (b=0; b<nslices; b++) {
      for (r=0; r<nrows; r++) {
	for (c=0; c<ncols; c++) {
	  u = VPixel(t_image,b,r,c,VFloat);
	  if (ABS(u) >= tiny) {
	    VPixel(mask,b,r,c,VBit) = 1;
	    nvox++;
	  }
	}
      }
    }

    if(nvox == 0)
      VError("TFCE: Number of voxels!=0 is zero.");
    /* voxel addresses */
    map = VCreateImage(1,3,nvox,VShortRepn);
    if (map == NULL) VError(" error allocating addr map");
    VFillImage(map,VAllBands,0);
    i = 0;
    for (b=0; b<nslices; b++) {
      for (r=0; r<nrows; r++) {
	for (c=0; c<ncols; c++) {
	  if (VGetPixel(mask,b,r,c) < 0.5) continue;
	  VPixel(map,0,0,i,VShort) = b;
	  VPixel(map,0,1,i,VShort) = r;
	  VPixel(map,0,2,i,VShort) = c;
	  i++;
	}
      }
    }
    VDestroyImage(mask);
  }

  /* stratification */
  /* set z strata */
  size_t nvox = VImageNColumns(map);
  double z;

  /* get an idea of z-value distribution */
  double zmax=0;
  for (i=0; i<nvox; i++) {
    b = VPixel(map,0,0,i,VShort);
    r = VPixel(map,0,1,i,VShort);
    c = VPixel(map,0,2,i,VShort);
    z = VPixel(t_image,b,r,c,VFloat);
    if (z > zmax) zmax = z;
  }

  /* ini strata */
  double step = zmax / (double)nstrata;
  double *stratum = (double *) VCalloc(nstrata+1,sizeof(double));
  for (j=0; j<=nstrata; j++) stratum[j] = step*j;

  float *clustersize = (float *) VCalloc(nvox,sizeof(float));
  if (!clustersize) VError(" err allocating clustersize");
  float *cluster_cont = (float *) VCalloc(nvox,sizeof(float));
  if (!cluster_cont) VError(" err allocating cluster_cont");
  for (i=0; i<nvox; i++) {
    clustersize[i] = 0;
    cluster_cont[i] = 0;
  }

  /* output, create TFCE image */
  VImage tfce  = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  if (!tfce) VError(" err allocating tfce image");
  VFillImage(tfce,VAllBands,0);

  /* ini images */
  VImage bin_image = VCreateImage(nslices,nrows,ncols,VBitRepn);
  if (!bin_image) VError(" err allocating bin_image image");
  VImage label_image = VCreateImage(nslices,nrows,ncols,VShortRepn);
  if (!label_image) VError(" err allocating label_image image");

  /* for each stratum */
  double *strat_sq = (double *)VCalloc(nstrata,sizeof(double));
  for (j=0; j<nstrata; j++)
    strat_sq[j] = stratum[j] * stratum[j];
  for (j=0; j<nstrata; j++) {
    VFillImage(bin_image,VAllBands,0);
    for (i=0; i<nvox; i++) {
      b = VPixel(map,0,0,i,VShort);
      r = VPixel(map,0,1,i,VShort);
      c = VPixel(map,0,2,i,VShort);
      z = VPixel(t_image,b,r,c,VFloat);
      if (z > stratum[j]) {
	VPixel(bin_image,b,r,c,VBit) = 1;
      }
    }

    /* get connected components (clusters) */
    long nl=0;
    VLabelImage3d(bin_image,label_image,26,VShortRepn,&nl);
    if (nl < 1) continue;   /* no voxels in this stratum found */

    /* compute cluster sizes */
    int max_k = 0;
    for (i=0; i<nvox; i++) clustersize[i] = 0;
    for (i=0; i<nvox; i++) {
      b = VPixel(map,0,0,i,VShort);
      r = VPixel(map,0,1,i,VShort);
      c = VPixel(map,0,2,i,VShort);
      k = VPixel(label_image,b,r,c,VShort);
      if (k > 0) clustersize[k]++;
      if (k > max_k) max_k = k;
    }

    /* compute contribution of the cluster to the voxels in it */
    for (i=0; i<=max_k; i++)
      cluster_cont[i] = sqrt(clustersize[i]) * strat_sq[j];

    /* add cluster contribution to each voxel */
    for (i=0; i<nvox; i++) {
      b = VPixel(map,0,0,i,VShort);
      r = VPixel(map,0,1,i,VShort);
      c = VPixel(map,0,2,i,VShort);
      k = VPixel(label_image,b,r,c,VShort);
      VPixel(tfce,b,r,c,VFloat) += cluster_cont[k];
    }
  }

  /* thickness correction */
  for (i=0; i<nvox; i++) {
    b = VPixel(map,0,0,i,VShort);
    r = VPixel(map,0,1,i,VShort);
    c = VPixel(map,0,2,i,VShort);
    k = VPixel(label_image,b,r,c,VShort);
    VPixel(tfce,b,r,c,VFloat) *= step;
  }

  VDestroyImage(bin_image);
  VDestroyImage(label_image);
  VFree(clustersize);
  VFree(cluster_cont);
  VFree(stratum);
  VFree(strat_sq);

  return tfce;
}


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_sf_zeta.h>

/* Calculate the Hurwitz zeta function by invoking the GNU scientific
   library */
/* Only intended to be used with R; not the most elegant integration but it
   may do for now */

/* Two arguments, the power used in the sum (s) and the baseline value (q) */

double s, q, zeta; /* arguments to zeta, value */
char *program_name; /* name program is invoked under, for errors */

int main(int argc, char* argv[]) {
  void usage(void);	/* Warn users about proper usage */

  program_name = argv[0];
  if (argc != 3) {
    usage();
  }
  s = atof(&argv[1][0]);
  if (s <= 1.0) {
    usage();
  }
  q = atof(&argv[2][0]);
  if (q <= 0.0) {
    usage();
  }
  zeta = gsl_sf_hzeta(s,q);
  printf("%.18e\n",zeta);
  return(0);
}

void usage(void) {
  (void) fprintf(stderr, "Usage is %s [floating-point exponent > 1] [floating-point additive constant > 0]\n", program_name);
  exit(8);
}


/**
  * Copyright 2016 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
  *
  * Matrix Market Suite 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 Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

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

#include <cblas.h>

#include "DMxV.h"
#include "basic.h"

void usageDMxV(){

	fprintf(stderr, "\n");
	fprintf(stderr, "Usage: MM-Suite DMxV [options] <input-matrix> <input-vector>\n");
	fprintf(stderr, "\nInput/output options:\n\n");
	fprintf(stderr, "       -o STR        Output file name. Default: stdout\n");
	fprintf(stderr, "       -r            Input format is row per line. Default: False\n");
	fprintf(stderr, "\nParameters options:\n\n");
	fprintf(stderr, "       -a DOUBLE     Alpha. Default: 1.0\n");
	fprintf(stderr, "       -b DOUBLE     Beta. Default: 0.0\n");
	fprintf(stderr, "\nPerformance options:\n\n");
	fprintf(stderr, "       -t INT        Number of threads to use in OpenBLAS. Default: 1\n");
	fprintf(stderr, "\n");

}

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

	int 			ret_code = 1;
	int 			option;

	unsigned long 		*II;
	unsigned long 		*J;
	double 			*values;

	unsigned long 		M;
	unsigned long 		N;
	unsigned long long 	nz;

	double 			*vectorValues;
	unsigned long 		M_Vector;
	unsigned long 		N_Vector;
	unsigned long long 	nz_vector;

	char			*outputFileName = NULL;

	char			*inputMatrixFile = NULL;
	char			*inputVectorFile = NULL;
	char			*outputVectorFile = NULL;

	int			inputFormatRow = 0;
	int			basicOps = 0;
	int			numThreads = 1;

	double			alpha = 1.0;
	double			beta = 0.0;

	while ((option = getopt(argc, argv,"ero:b:a:t:")) >= 0) {
		switch (option) {
			case 'o' :
				//free(outputFileName);

				outputFileName = (char *) malloc(sizeof(char)*strlen(optarg)+1);
				strcpy(outputFileName,optarg);

				break;

			case 'r':
				inputFormatRow = 1;
				break;

			case 'e':
				basicOps = 1;
				break;

			case 'b':
				beta = atof(optarg);
				break;

			case 'a':
				alpha = atof(optarg);
				break;

			case 't':
				numThreads = atoi(optarg);
				break;

			default: break;
		}

	}

	if ((optind + 3 != argc) && (optind + 2 != argc)) {
		usageDMxV();
		return 0;
	}

	openblas_set_num_threads(numThreads);

	if(optind + 3 == argc) { //We have an output vector

		outputVectorFile = (char *)malloc(sizeof(char)*strlen(argv[optind+2])+1);
		strcpy(outputVectorFile,argv[optind+2]);
	}

	if(outputFileName == NULL) {
		outputFileName = (char *) malloc(sizeof(char)*7);
		sprintf(outputFileName,"stdout");
	}

	inputMatrixFile = (char *)malloc(sizeof(char)*strlen(argv[optind])+1);

	if(inputMatrixFile == NULL) {
		fprintf(stderr, "[%s] Error reserving memory for input matrix file name\n",__func__);
		return 0;
	}

	inputVectorFile = (char *)malloc(sizeof(char)*strlen(argv[optind+1])+1);

	if(inputVectorFile == NULL) {
		fprintf(stderr, "[%s] Error reserving memory for input vector file name\n",__func__);
		return 0;
	}

	strcpy(inputMatrixFile,argv[optind]);
	strcpy(inputVectorFile,argv[optind+1]);

	//Read matrix
	if(inputFormatRow){

		if(!readDenseCoordinateMatrixRowLine(inputMatrixFile,&II,&J,&values,&M,&N,&nz)){
			usageDMxV();
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return 0;
		}

	}
	else {
		if(!readDenseCoordinateMatrix(inputMatrixFile,&II,&J,&values,&M,&N,&nz)){
			usageDMxV();
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return 0;
		}
	}

	//Read vector
	if(!readDenseVector(inputVectorFile, &vectorValues,&M_Vector,&N_Vector,&nz_vector)){
		usageDMxV();
		fprintf(stderr, "[%s] Can not read Vector\n",__func__);
		return 0;
	}

	/*
	void cblas_dgemv(const enum CBLAS_ORDER order,
                 const enum CBLAS_TRANSPOSE TransA, const int M, const int N,
                 const double alpha, const double *A, const int lda,
                 const double *X, const int incX, const double beta,
                 double *Y, const int incY);
                 */

        double *result=(double *) malloc(nz_vector * sizeof(double));

        //Read output vector if any
	if(outputVectorFile != NULL) {
		if(!readDenseVector(outputVectorFile, &result,&M_Vector,&N_Vector,&nz_vector)){
			usageDMxV();
			fprintf(stderr, "[%s] Can not read Vector %s\n",__func__, outputVectorFile);
			return 0;
		}
	}

	//cblas_dgemv(CblasColMajor,CblasNoTrans,M,N,1.0,values,N,vectorValues,1,0.0,result,1);
	double t_real = realtime();
	if(basicOps){
		mms_dgemv(M, N, alpha, values, vectorValues,beta, result);
	}
	else{
		cblas_dgemv(CblasRowMajor,CblasNoTrans,M,N,alpha,values,N,vectorValues,1,beta,result,1);
	}

	fprintf(stderr, "\n[%s] Time spent in cblas_dgemv: %.6f sec\n", __func__, realtime() - t_real);

	writeDenseVector(outputFileName, result,M_Vector,N_Vector,nz_vector);

	return ret_code;
}


/* ============================================================ *
 * lensing.h							*
 *								*
 * Martin Kilbinger, Karim Benabed 2006-2012			*
 * ============================================================ */

#ifndef __LENSING_H
#define __LENSING_H

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

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_rng.h>

#include "errorlist.h"
#include "maths.h"
#include "io.h"
#include "mvdens.h"
#include "par.h"

#include "hod.h"
#include "halomodel.h"
#include "cosmo.h"
#include "nofz.h"
#include "decomp_eb.h"
#include "reduced_fit.h"

/* Dimensions of interpolation tables */
/* N_s was increased from 200 to 400, for linear tabulation of P_kappa */
#define N_s     400
#define N_theta 100

/* Ranges of interpolation table for convergence power spectrum. *
 * Power-law extrapolation outside these ranges.		 */
#define s_min     1.0e-2
#define s_max     1.0e6

/* Ranges of interpolation table for reduced-shear correction    *
 * power spectrum. No extrapolation outside these ranges.	 */
#define ELL_MIN_REDUCED            0.1
#define ELL_MAX_REDUCED            2.0e5
#define THETA_P_MIN_REDUCED        (0.1*arcmin)
#define THETA_M_MIN_REDUCED        (0.5*arcmin)
#define THETA_MAP_MIN_REDUCED      (0.2*arcmin)
#define THETA_GSQR_MIN_REDUCED     (0.1*arcmin)
#define THETA_MAPGAUSS_MIN_REDUCED (0.1*arcmin)
#define THETA_MAX_REDUCED          (1000.0*arcmin)

#define NELL_REDUCED 50

#define lensing_base                     -1400
#define lensing_inconsistent             -1 + lensing_base
#define lensing_baryon_fraction          -2 + lensing_base
#define lensing_tomoij			 -3 + lensing_base
#define lensing_initialised              -4 + lensing_base
#define lensing_unknown                  -5 + lensing_base
#define lensing_pm                       -6 + lensing_base
#define lensing_type                     -7 + lensing_base
#define lensing_fastxi                   -8 + lensing_base
#define lensing_nperm                    -9 + lensing_base
#define lensing_range                   -10 + lensing_base
#define lensing_cosebi_n_max            -11 + lensing_base
#define lensing_ia                      -12 + lensing_base
#define lensing_angle_format            -13 + lensing_base
#define lensing_nzbin                   -14 + lensing_base

/* If Ob/Oc > BARYON_FRAC, chi2 produces an error */
#define BARYON_FRAC 0.75

/* Intrinsic alignment, constant amplitude C_1 * rho_crit, *
 * with C_1 = 5e-14 h^2 Mpc^3/M_sol.                       */
#define ia_c1_rho_crit     0.0134

typedef enum {xipm, xip, xim, map2poly, map2gauss, gsqr, decomp_eb, nofz, pkappa, map3gauss,
	      map3gauss_diag, map2gauss_map3gauss_diag, map2gauss_map3gauss,
	      decomp_eb_map3gauss_diag, decomp_eb_map3gauss}
  lensdata_t;
#define slensdata_t(i) ( \
 i==xipm      ? "xipm" : \
 i==xip       ? "xip"  : \
 i==xim       ? "xim"  : \
 i==map2poly  ? "map2poly" : \
 i==map2gauss ? "map2gauss" : \
 i==gsqr      ? "gsqr" : \
 i==decomp_eb ? "decomp_eb" : \
 i==nofz      ? "nofz" : \
 i==pkappa    ? "pkappa" : \
 i==map3gauss ? "map3gauss" : \
 i==map3gauss_diag ? "map3gauss_diag" : \
 i==map2gauss_map3gauss_diag ? "map2gauss_map3gauss_diag" : \
 i==map2gauss_map3gauss ? "map2gauss_map3gauss" : \
 i==decomp_eb_map3gauss_diag ? "decomp_eb_map3gauss_diag" : \
 i==decomp_eb_map3gauss ? "decomp_eb_map3gauss" : \
 "")
#define Nlensdata_t 15

typedef enum {decomp_eb_none, FK10_SN, FK10_FoM_eta10, FK10_FoM_eta50, COSEBIs_log} decomp_eb_filter_t;
#define sdecomp_eb_filter_t(i) (		\
 i==decomp_eb_none ? "none" : \
 i==FK10_SN        ? "FK10_SN" : \
 i==FK10_FoM_eta10 ? "FK10_FoM_eta10" : \
 i==FK10_FoM_eta50 ? "FK10_FoM_eta50" : \
 i==COSEBIs_log    ? "COSEBIs_log" : \
 "")
#define Ndecomp_eb_filter_t 5

/* The following arrays are defined in decomp_eb.c */
extern const double a_FK10_SN[], a_FK10_FoM_eta10[], a_FK10_FoM_eta50[];
// r_COSEB[];

typedef enum {angle_center, angle_mean, angle_wlinear, angle_wquadr} lensformat_t;
#define slensformat_t(i) ( \
 i==angle_center  ? "angle_center" : \
 i==angle_mean    ? "angle_mean" : \
 i==angle_wlinear ? "angle_wlinear" : \
 i==angle_wquadr  ? "angle_wquadr" : \
 "")
#define Nlensformat_t 4

typedef enum {cov_const, cov_ESH09} cov_scaling_t;
#define scov_scaling_t(i) ( \
  i==cov_const ? "cov_const" : \
  i==cov_ESH09 ? "cov_ESH09" : \
  "__undef__")
#define Ncov_scaling_t 2

typedef enum {reduced_none, reduced_K10} reduced_t;
#define sreduced_t(i) ( \
  i==reduced_none ? "none" : \
  i==reduced_K10  ? "K10" : \
  "")
#define Nreduced_t 2

/* Intrinsic alignment model */
typedef enum {ia_none, ia_HS04} ia_t;
#define sia_t(i) ( \
  i==ia_none ? "none" :	\
  i==ia_HS04 ? "HS04" : \
  "")
#define Nia_t 2

/* Bit-coded IA terms */
typedef enum {ia_undef, ia_GI_II, ia_only_GI, ia_only_II} ia_terms_t;
#define sia_terms_t(i) (      \
  i==ia_undef   ? "undef" :   \
  i==ia_GI_II   ? "GI_II" :   \
  i==ia_only_GI ? "only_GI" : \
  i==ia_only_II ? "only_II" : \
  "")
#define Nia_terms_t 4

typedef enum {second_order=2, third_order=3} order_t;

typedef enum {tomo_all, tomo_auto_only, tomo_cross_only} tomo_t;
#define stomo_t(i) ( \
 i==tomo_all        ? "tomo_all" : \
 i==tomo_auto_only  ? "tomo_auto_only" : \
 i==tomo_cross_only ? "tomo_cross_only" : \
 "")
#define Ntomo_t 3

typedef struct {
  int n_max;
  double th_min, th_max;
  char path[1024];
} cosebi_info_t;

typedef struct {

  /* Basic cosmology */
  cosmo *cosmo;

  /* Redshift distribution(s) */
  redshift_t *redshift;

  /* Tomography type */
  tomo_t tomo;

  /* Reduced-shear correction */
  reduced_t reduced;
  double q_mag_size;  /* q_mag_size = 2(alpha+beta-1),         *
		       * alpha, beta: slopes of number density *
		       * with flux (alpha), size (beta)        */

  /* Intrinsic aligmnent */
  ia_t ia;
  double A_ia;           /* IA amplitude */
  ia_terms_t ia_terms;  /* Bit-coded terms, GG=1, GI=2, II=4 */

  /* Halomodel stuff (only initialised if cosmo->nonlinear=halodm) */
  cosmo_hm *hm;

  /* ============================================================ *
   * Precomputed stuff.						  *
   * ============================================================ */

  interTable **g_source;
  interTable **Pshear, **Pg1;

  /* Shear second-order functions */
  interTable **xiP, **xiM, **gamma, **map_gauss, **map_poly;
  double *c_cosebi, psimin_cosebi, psimax_cosebi;
  int N_cosebi;

} cosmo_lens;

typedef struct {
  double r;
  cosmo_lens* self;
} cosmo_lensANDdouble;

typedef struct {
  int i;
  double r;
  cosmo_lens *self;
} cosmo_lensANDintANDdouble;

typedef struct {
  int i, j;
  double r;
  cosmo_lens *self;
} cosmo_lensANDiid;

typedef struct {
  int i, j, t;
  double r;
  cosmo_lens *self;
} cosmo_lensANDiiid;

typedef struct {
  int i_bin, j_bin, pm, n;
  const double *c;
  double thmin;
  cosmo_lens *self;
  error **err;
} cosmo_lensANDextra;

typedef struct {
   int Ntheta, Nzbin;  /* Number of angular and redshift bins */
   int Ntheta2;        /* For combined 2nd and 3rd-order */
   int Nzcorr;         /* Number of z-correlations, Nzcorr=Nzbin*(Nzbin+1)/2 */
   int n;              /* Number of total entries in data vector, n=Ntheta*Nzcorr */
   double *theta;      /* n-dimensional vector of angular scales */
   double *theta2;     /* For angle_range lensformats: (theta,theta2) = (lower,upper) bin limits */
   double *data;       /* n-dimensional data vector */
   double *var;        /* n-dimensional vector with variance */
   double *cov[3];     /* Maximum three nxn-dimensional covariance matrix */
   double a1, a2;      /* Coefficients for 'angle_wquadr' */
   double lndetC;
   int usecov;
   lensdata_t type;
   lensformat_t format;
   order_t order;
   decomp_eb_filter_t decomp_eb_filter;
   cov_scaling_t cov_scaling;
   cosmo_lens *fiducial;   /* Needed for ESH09 cov scaling */
} datcov;

/* ============================================================ *
 * Initialisation.						*
 * ============================================================ */

cosmo_lens *init_parameters_lens(double OMEGAM, double OMEGAV, double W0_DE, double W1_DE,
				 double *W_POLY_DE, int N_POLY_DE,
				 double H100, double OMEGAB, double OMEGANUMASS,
				 double NEFFNUMASS, double NORM, double NSPEC,
				 int Nzbin, const int *Nnz, const nofz_t *nofz, double *par_nz,
				 nonlinear_t NONLINEAR, transfer_t TRANSFER,
				 growth_t GROWTH, de_param_t DEPARAM,
				 norm_t normmode, tomo_t TOMO, reduced_t REDUCED, double Q_MAG_SIZE,
				 ia_t IA, ia_terms_t ia_terms, double A_IA, error **err);

void consistency_parameters_lens(const cosmo_lens *self, error **err);
cosmo_lens* copy_parameters_lens_only(cosmo_lens* source, error **err);
cosmo_lens* copy_parameters_lens(cosmo_lens* source, sm2_error **err);
void updateFrom_lens(cosmo_lens* avant, cosmo_lens* apres, error **err);
void copy_parameters_lenshm_cosmo(cosmo_lens *model, error **err);
void read_cosmological_parameters_lens(cosmo_lens **self, FILE *F, error **err);
cosmo_lens* set_cosmological_parameters_to_default_lens(error **err);
void free_parameters_lens(cosmo_lens** self);
void dump_param_lens(cosmo_lens* self, FILE *F, int wnofz, error **err);

/* ============================================================ *
 * Lensing functions.						*
 * ============================================================ */

/* Projection */
double int_for_g(double aprime, void *intpar, error **err);
double g_source(cosmo_lens*, double a, int n_bin, error **err);
double G(cosmo_lens* self, double a, int n_bin, error **err);
double int_for_p_2(double a, void *intpar,error **err);
double P_NL_tot(cosmo_lens *self, double a, double k, error **err);
double Pshear(cosmo_lens *self, double a, int i_bin, int j_bin, error **err);
double P_projected_kappa(void *self, double l, int i_bin, int j_bin, error **err);
double int_over_P_kappa(cosmo_lens *self, funcwithpars int_for_p, void *intpar, error **err);

double int_for_p_GI(double a, void *intpar, error **err);
double int_for_p_II(double a, void *intpar, error **err);

/* Reduced-shear correction (K10) */
extern const int parameter[M_PAR];
cosmo *set_cosmological_parameters_to_WMAP7(const redshift_t *nofz, tomo_t tomo, error **err);
double *par_to_pointer(cosmo *self, par_t par, error **err);
void fill_dpar(cosmo *model, cosmo *wmap7, double *dpar, error **err);
double Fbar(cosmo_lens *self, double a, int m_bin, int n_bin, error **err);
void fill_Fbar_array(cosmo_lens *self, double *fbar, int m_bin, int n_bin, double amin, int N_a,
		     double da, error **err);
void fill_dFbar_dp_array(cosmo_lens *self, par_t par, double *dfbar_dp, int m_bin, int n_bin, double amin,
			 int N_a, double da, error **err);
double Pg1(cosmo_lens *self, double s, int i_bin, int j_bin, error **err);

/* Second-order shear functions */
double xi(cosmo_lens*, int pm, double theta, int i_bin, int j_bin, error **err);
double gamma2(cosmo_lens*, double theta, int i_bin, int j_bin, error **err);
double map2_poly(cosmo_lens*, double theta, int i_bin, int j_bin, error **err);
double map2_gauss(cosmo_lens*, double theta, int i_bin, int j_bin, error **err);
double RR(cosmo_lens *lens, double THETA_MIN, double THETA_MAX, const double *a, int N,
	  poly_t poly, int pm, error **err);
double E_cosebi(cosmo_lens *lens, int n, double Psimin, double Psimax, int i_bin, int j_bin,
		const char *path, double *B_cosebi, error **err);
double RR_cosebi(cosmo_lens *lens, double THETA_MIN, double THETA_MAX, int i_bin, int j_bin,
		 int n, int pm, error **err);
double dRR_cosebi_dz_MC(double *z, int ndim, void *intpar);
double dRR_cosebi_dz(double z, void *intpar, error **err);
double int_for_map2_slow(double ell, void *intpar, error **err);
double map2_slow(cosmo_lens *self, double theta, tpstat_t tpstat, int i_bin, int j_bin, error **err);

/* Reading data files */
datcov *init_data_cov_tomo(char* dataname, char *dataname2, char** covname_ptr, lensdata_t type,
			   decomp_eb_filter_t decomp_eb_filter,
			   lensformat_t format, double corr_invcov,
			   double a1, double a2, order_t order,
			   cov_scaling_t cov_scaling, error **err);
datcov *init_datcov_for_cov_only(int Nzbin, int Ntheta, error **err);

void del_data_cov(datcov** dc);
void read_data_tomo(datcov *dc, char data_name[], int Nzbin, order_t order, error **err);
void read_cov_tomo(datcov* dc, char cov_name[], int icov, error **err);
void datcov2xipm(const datcov *dc, int i_bin, int j_bin, double **xip, double **xim, double **theta,
		 double **theta2, int *N, error **err);

//void read_cov(datcov* dc, char cov_name[], error **err);
void read_cov_col(datcov *dc, char cov_name[], error **err);

int get_pm(lensdata_t type, int i, int Ntheta, error **err);
int find_bin(double x, const double *list, int N, int prev, error **err);
void scale_cosmic_variance_ESH09(cosmo_lens *model, gsl_matrix *cov, const datcov *dc, error **err);
void scale_mixed_ESH09(const cosmo_lens *model, gsl_matrix *cov, const datcov *dc, error **err);
double lensing_signal(cosmo_lens *model, double theta, int i_bin, int j_bin, lensdata_t type,
		      decomp_eb_filter_t decomp_eb_filter, const cosebi_info_t *cosebi_info, error **err);
double chi2_lensing(cosmo_lens* csm, datcov* dc, int return_model, double **model_array, int *Nmodel,
		    const cosebi_info_t *cosebi_info, error **err);

/* Some third-order stuff which is called from lensing.c */
int Nperm_to_Ntheta(int Nperm, error **err);
void read_data_3rd(datcov *dc, char data_name[], int Nzbin, error **err);
void read_data_2nd_3rd(datcov *res, char *dataname, char *dataname2, error **err);

#define CHANGE(fct) int change_##fct(cosmo_lens*, cosmo_lens*)
CHANGE(g_source);
CHANGE(Pshear);
CHANGE(xi);
CHANGE(gamma2);
CHANGE(map2);
#undef CHANGE

#endif /* __LENSING_H */


// Data Structure and Routines for peak fiting

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlin.h>

#include <errno.h>

#include "grid.h"
#include "grid_operations.h"
#include "minmax.h"
#include "point.h"

#ifndef _FITTOFUNCTION_H_
#define _FITTOFUNCTION_H_

typedef  struct {
  size_t n;
  double * y;
} ObservedValues ;

void fitToFunctionLorentz(Grid *image, double *fitx, double *fity,
		   double *background, double *intens,
		   double *widthx, double *widthy, double *tilt,double *chisq);

void fitToFunctionGauss(Grid *image, double *fitx, double *fity,
		   double *background, double *intens,
		   double *widthx, double *widthy, double *tilt,double *chisq);

#endif


/* hmm.c v0.0

 	Andrew D. Kern	8/24/05
*/

#include "hmm.h"
#include "adkGSL.h"
#include <math.h>
#include <assert.h>
#include <string.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_randist.h>

/* returns a pointer to a new instance of an HMM.
Currently an HMM is defined by a transition matrix (pMatrix),
and a vector representing the starting probabilities.
use of initHMM() fills the log score matrices. 	*/

HMM* newHMM(gsl_matrix *pMatrix, gsl_vector *piStart, gsl_vector *piEnd, gsl_vector *otherData ) {
  HMM *hmm = (HMM*)malloc(sizeof(HMM));
  int i;

  hmm->transition_matrix = pMatrix;
  hmm->piStart = piStart;
  hmm->piEnd = piEnd;
  hmm->nstates = pMatrix->size1;
  hmm->transition_score_matrix = NULL;
  hmm->piStart_scores = NULL;
  hmm->piEnd_scores = NULL;
  hmm->otherData = otherData;

  /* if piStart are NULL, make them uniform */
  if (piStart == NULL) {
    hmm->piStart = gsl_vector_alloc(pMatrix->size1);
    for (i = 0; i < pMatrix->size1; i++)
      gsl_vector_set(hmm->piStart, i, 1.0/pMatrix->size1);
  }
  if (piEnd == NULL) {
    hmm->piEnd = gsl_vector_alloc(pMatrix->size1);
    for (i = 0; i < pMatrix->size1; i++)
      gsl_vector_set(hmm->piEnd, i, 1.0/pMatrix->size1);
  }

  return hmm;
}

/* initialize an HMM, this makes sure that things are kosher,
it checks that the row sums of the pMatrix == 1, that the sum
of the piStart vector == 1. then it initializes scores */

void initHMM(HMM *hmm){
	gsl_matrix *pMatrixLogs;
	gsl_vector *piStartLogs, *piEndLogs;
	int i, j;
	double piEndSum, piStartSum, prob;

	//check piStartSum == 1
	piStartSum = gsl_vector_sum(hmm->piStart, hmm->nstates);
	assert(piStartSum == 1.0);

	//make log transformed start probs
	piStartLogs = gsl_vector_alloc(hmm->nstates);
	for (i = 0; i < hmm->nstates; i++){
		prob = gsl_vector_get(hmm->piStart, i);
		gsl_vector_set(piStartLogs, i, log(prob));
		}

	//set hmm piEnd_scores
	hmm->piStart_scores = piStartLogs;

	//check piEndSum == 1
	piEndSum = gsl_vector_sum(hmm->piEnd, hmm->nstates);
	assert(piEndSum == 1.0);

	//make log transformed start probs
	piEndLogs = gsl_vector_alloc(hmm->nstates);
	for (i = 0; i < hmm->nstates; i++){
		prob = gsl_vector_get(hmm->piEnd, i);
		gsl_vector_set(piEndLogs, i, log(prob));
		}

	//set hmm piEnd_scores
	hmm->piEnd_scores = piEndLogs;

	//check row sums for markov matrix
	for(i = 0; i < hmm->nstates; i++){
		assert(gsl_matrix_row_sum(hmm->transition_matrix, i, hmm->nstates) == 1.0);
		}
	//make log transformed transition matrix
	pMatrixLogs = gsl_matrix_alloc(hmm->nstates, hmm->nstates);
	for (i = 0; i < hmm->nstates; i++){
		for (j = 0; j < hmm->nstates; j++){
			prob = gsl_matrix_get(hmm->transition_matrix, i, j);
			gsl_matrix_set(pMatrixLogs, i, j, log(prob));
			}
		}

	//set hmm transition_score_matrix
	hmm->transition_score_matrix = pMatrixLogs;
}

/* frees an entire hmm */
void freeHMM(HMM *hmm){

	if (hmm->transition_matrix)
		gsl_matrix_free(hmm->transition_matrix);
	if (hmm->transition_score_matrix)
		gsl_matrix_free(hmm->transition_score_matrix);
	if (hmm->piStart)
		gsl_vector_free(hmm->piStart);
	if (hmm->piStart_scores)
		gsl_vector_free(hmm->piStart_scores);
	if (hmm->piEnd)
		gsl_vector_free(hmm->piEnd);
	if (hmm->piEnd_scores)
		gsl_vector_free(hmm->piEnd_scores);
	if (hmm->otherData)
		gsl_vector_free(hmm->otherData);
	free(hmm);
}

/*output hmm values: nstates, transition matrix, piStarts */
void printHMM(HMM *hmm){
  printf("hmm output\nstates: %d \ntransition matrix:\n", hmm->nstates);
  gsl_matrix_fprintf(stdout, hmm->transition_matrix, "%f");
  printf("piStart vector:\n");
  gsl_vector_fprintf(stdout, hmm->piStart, "%f");
  printf("piEnd vector:\n");
  gsl_vector_fprintf(stdout, hmm->piEnd, "%f");
  if (hmm->transition_score_matrix != NULL){
    printf("transition matrix (logs):\n");
    gsl_matrix_fprintf(stdout, hmm->transition_score_matrix, "%f");
  }
  if (hmm->piStart_scores != NULL){
    printf("piStart vector (logs):\n");
    gsl_vector_fprintf(stdout, hmm->piStart_scores, "%f");
  }
  if (hmm->piEnd_scores != NULL){
    printf("piEnd vector (logs):\n");
    gsl_vector_fprintf(stdout, hmm->piEnd_scores, "%f");
  }
  if (hmm->otherData != NULL){
    //    printf("other data:\n");
    //gsl_vector_fprintf(stdout, hmm->otherData, "%f");
  }

}

/*output transition matrix of hmm values to file */
void printTransitions(HMM *h, char *outfileName){
  FILE *outfile;
  int i,j;

   outfile = fopen(outfileName, "w");
   if (outfile == NULL){
     fprintf(stderr,"Error opening outfile! ARRRRR!!!!\n");
     exit(1);
   }
   for(i = 0; i < h->nstates; i++){
     for(j = 0; j < h->nstates; j++){
       fprintf(outfile,"%f\t",gsl_matrix_get(h->transition_matrix,i,j));
     }
     fprintf(outfile,"\n");
   }
   fclose(outfile);
}

/* this normalizes the transition matrix so that rows sum to one */
void hmm_normalize_transitions(HMM *hmm){
  int i, j;
  double rowSum, tempVal;

  for(i=0; i < hmm->nstates; i++){
    rowSum = 0;
    for(j=0; j < hmm->nstates; j++){
      rowSum += gsl_matrix_get(hmm->transition_matrix,i,j);
    }
    if(rowSum == 0){
      gsl_matrix_set(hmm->transition_matrix, i, i, 1.0);
    }
    else{
      for(j= 0; j < hmm->nstates;j++){
	tempVal =  gsl_matrix_get(hmm->transition_matrix, i, j) / rowSum;
	gsl_matrix_set(hmm->transition_matrix, i, j, tempVal);
      }
    }
  }
}

void hmm_logify_transitions(HMM *hmm){
  int i, j;
  double prob;
  //set log transition scores
  for (i = 0; i < hmm->nstates; i++){
    for (j = 0; j < hmm->nstates; j++){
      prob = gsl_matrix_get(hmm->transition_matrix, i, j);
      gsl_matrix_set(hmm->transition_score_matrix, i, j, log(prob));
    }
  }
  //set log start and end scores
  for (i = 0; i < hmm->nstates; i++){
    prob = gsl_vector_get(hmm->piStart, i);
    gsl_vector_set(hmm->piStart_scores, i, log(prob));
    prob = gsl_vector_get(hmm->piEnd, i);
    gsl_vector_set(hmm->piEnd_scores, i, log(prob));
  }
}

/* forward algorithm- this fills a matrix (alphas) of Nstates * L observations
with the forward log probabilities. a matrix of emission probs (log!!!) with the same
dimensions as the forward matrix must be allocated and passed to this function.
returns probFor. */

double forwardAlg(HMM *hmm, gsl_matrix *alphas, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
	int i, j, power, last;
	double prob, emit, prevSum, pFor;
	gsl_vector_view prevCol;
	gsl_vector *tempProbs;
	gsl_matrix *logTransitions;

	assert(length > 0 && hmm != NULL);
	assert(alphas->size1 == hmm->nstates && alphas->size2 == length);
	assert(emisLogs->size1 == hmm->nstates && emisLogs->size2 == length);

//initialization
	for (i = 0; i < hmm->nstates; i++){
		prob = gsl_vector_get(hmm->piStart_scores, i);
		emit = gsl_matrix_get(emisLogs, i, 0);
		gsl_matrix_set(alphas, i, 0, (prob + emit));
	}

//are we using powers of the transition matrix? if so check to see we have location data
	if (transitionPowerFlag){
		assert(hmm->otherData != NULL);
	//set "last" equal to the first location (stored in otherData) here's a totally non general assumption... FIX ME!!!
		last = gsl_vector_get(hmm->otherData,0);
	}
	else {
		last = 0; /* not used */
	}

//fill em up!
	for (j = 1; j < length; j++){			//iterate over obs
		for (i = 0; i < hmm->nstates; i++){		//iterate over states
			emit = gsl_matrix_get(emisLogs, i, j);
			/* are we taking powers here? */
			if (transitionPowerFlag){
			//get power from data
				power = gsl_vector_get(hmm->otherData,j) - last;

				//get power of matrix in log scale
				logTransitions = gsl_matrix_power_logs(hmm->transition_matrix, power);
				prevSum = forwardSumPrevious(hmm, alphas, logTransitions, i, j);
				gsl_matrix_free(logTransitions);
			}
			/* no powers */
			else{
				prevSum = forwardSumPrevious(hmm, alphas, hmm->transition_score_matrix, i, j);
			//fprintf (stderr,"forwardAlg state %2d, index %2d, log.prevSum %g, emit %g\n",i,j,prevSum,emit);
			}
			gsl_matrix_set(alphas, i, j, (emit + prevSum));
		}
		if (transitionPowerFlag){
			last = gsl_vector_get(hmm->otherData,j);
		}
	}
//return P(x)
	tempProbs = gsl_vector_alloc(hmm->nstates);
	prevCol = gsl_matrix_column(alphas,  length - 1);
	for(i = 0; i < hmm->nstates; i++){
		gsl_vector_set(tempProbs, i, gsl_vector_get(&prevCol.vector, i));
	}
	pFor = log_sum(tempProbs);
	gsl_vector_free(tempProbs);
	return(pFor);
}

/* forward algorithm offset- identical to the normal forward algorithm except starts at an arbitrary
offset with respect to the observations. this is useful for getting probabilities from path subsets */

double forwardAlgOffset(HMM *hmm, gsl_matrix *alphas, gsl_matrix *emisLogs, int length, int offset, int transitionPowerFlag){
  int i, j, power, last;
  double prob, emit, prevSum, pFor;
  gsl_vector_view prevCol;
  gsl_vector *tempProbs;
  gsl_matrix *logTransitions;

  assert(length > 0 && hmm != NULL);
  assert(alphas->size1 == hmm->nstates && alphas->size2 == length);

  //initialization
  for (i = 0; i < hmm->nstates; i++){
    prob = gsl_vector_get(hmm->piStart_scores, i);
    emit = gsl_matrix_get(emisLogs, i, offset);
    gsl_matrix_set(alphas, i, 0, (prob + emit));
  }

  //are we using powers of the transition matrix? if so check to see we have location data
  if (transitionPowerFlag){
    assert(hmm->otherData != NULL);
    //set "last" equal to the first location (stored in otherData) here's a totally non general assumption... FIX ME!!!
    last = gsl_vector_get(hmm->otherData,offset);
  }
  else {
      last = 0; /* not used */
  }

  //fill em up!
  for (j = 1; j < length; j++){			//iterate over obs
    for (i = 0; i < hmm->nstates; i++){		//iterate over states
      emit = gsl_matrix_get(emisLogs, i, j + offset);
      /* are we taking powers here? */
      if (transitionPowerFlag){
	//get power from data
	power = gsl_vector_get(hmm->otherData,j + offset) - last;

	//get power of matrix in log scale
	logTransitions = gsl_matrix_power_logs(hmm->transition_matrix, power);
	prevSum = forwardSumPrevious(hmm, alphas, logTransitions, i, j);
	gsl_matrix_free(logTransitions);
      }
      /* no powers */
      else{
	prevSum = forwardSumPrevious(hmm, alphas, hmm->transition_score_matrix, i, j);
      }
      gsl_matrix_set(alphas, i, j, (emit + prevSum));
    }
    if (transitionPowerFlag){
	last = gsl_vector_get(hmm->otherData,j+offset);
    }
  }
  //return P(x)
  tempProbs = gsl_vector_alloc(hmm->nstates);
  prevCol = gsl_matrix_column(alphas,  length - 1);
  for(i = 0; i < hmm->nstates; i++){
    gsl_vector_set(tempProbs, i, gsl_vector_get(&prevCol.vector, i));
  }
  pFor = log_sum(tempProbs);
  gsl_vector_free(tempProbs);
  return(pFor);
}

/* backward algorithm- this fills a matrix (betas) of Nstates * L observations
with the backward log probabilities. a matrix of emission probs (log!!!) with the same
dimensions as the backward matrix must be allocated and passed to this function.
returns probBack. */

double backwardAlg(HMM *hmm, gsl_matrix *betas, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
  int i, j, last, power;
  gsl_vector_view nextCol;
  gsl_vector *tempProbs;
  gsl_matrix *logTransitions;
  double pBack;

  assert(length > 0 && hmm != NULL);
  assert(betas->size1 == hmm->nstates && betas->size2 == length);
  assert(emisLogs->size1 == hmm->nstates && emisLogs->size2 == length);

  //initialization
  for (i = 0; i < hmm->nstates; i++){
    //prob = gsl_vector_get(hmm->piEnd_scores, i);
    //emit = gsl_matrix_get(emisLogs, i, length - 1);
    gsl_matrix_set(betas, i, length - 1, 0);		//initialize to prob = 1.0, log = 0
  }

  //are we using powers of the transition matrix?
  if (transitionPowerFlag){
    assert(hmm->otherData != NULL);
    //set "last" equal to the last location (stored in otherData) here's a totally non general assumption... FIX ME!!!
    last = gsl_vector_get(hmm->otherData,length - 1);
  }
  else {
      last = 0; /* not used */
  }

  //fill em up!
  for (j =  length - 2; j >=0; j--){			//iterate over obs
    for (i = 0; i < hmm->nstates; i++){		//iterate over states
      /* are we taking powers here? */
      if (transitionPowerFlag){
	//get power from data
	power = last - gsl_vector_get(hmm->otherData,j);

	//get power of matrix in log scale
	logTransitions = gsl_matrix_power_logs(hmm->transition_matrix, power);
	gsl_matrix_set(betas, i, j, backwardSumNext(hmm, betas,logTransitions,emisLogs, i, j));
	gsl_matrix_free(logTransitions);
      }
      else{
	gsl_matrix_set(betas, i, j, backwardSumNext(hmm, betas,hmm->transition_score_matrix,emisLogs, i, j));
      }
    }
    if (transitionPowerFlag){
	last = gsl_vector_get(hmm->otherData,j);
    }
  }
  //return P(x)
  tempProbs = gsl_vector_alloc(hmm->nstates);
  nextCol = gsl_matrix_column(betas, 0);
  for(i = 0; i < hmm->nstates; i++){
    gsl_vector_set(tempProbs, i, (gsl_vector_get(&nextCol.vector, i) + gsl_matrix_get(emisLogs, i, 0) + gsl_vector_get(hmm->piStart_scores, i)));
  }
  pBack = log_sum(tempProbs);
  gsl_vector_free(tempProbs);
  return(pBack);
}

/* posterior probs- this fills a matrix (postProbMat) of nstates * L observations with the
posterior prob (not log !!) of being in that state at time i given the observations and the model.
calls forwardAlg and backwardAlg, checks that prob of observations determined by
forward and backward are (nearly) equal, and also (nearly) equal the sum of each column
before normalization to postProbMat.
a matrix of emission probs (log!!!) of dimensions nstates * L observations must be supplied. */

void posteriorProbs(HMM *hmm, gsl_matrix *postProbMat, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
	gsl_matrix *alphas, *betas;
	gsl_vector *tempVec;
	int i, j;
	double pFor, pBack, sumProb;

	//allocate matrices for forwards and backwards algs.
	alphas = gsl_matrix_alloc(hmm->nstates, length);
	betas = gsl_matrix_alloc(hmm->nstates, length);

	//run forwards/backwards
	pFor = forwardAlg(hmm, alphas, emisLogs, length, transitionPowerFlag);
	pBack = backwardAlg(hmm, betas, emisLogs, length, transitionPowerFlag);

	//check that Forward and Backward look okay
	//assert(fabs(pFor - pBack) < 0.01);

	//do the posterior probability thing- okay sir pull the switch
	tempVec = gsl_vector_alloc(hmm->nstates);
	for(j=0; j < length; j++){
		//first get sum for denominator
		for(i = 0; i < hmm->nstates; i++){
			gsl_vector_set(tempVec, i, (gsl_matrix_get(alphas,i,j) +  gsl_matrix_get(betas,i,j)));
		}
		sumProb = log_sum(tempVec);

		//check that sum looks like pFor
		assert(fabs(pFor - sumProb) < 0.01);

		//then calculate marginal and set value (not log)
		for(i = 0; i < hmm->nstates; i++){
			gsl_matrix_set(postProbMat, i, j, exp(gsl_matrix_get(alphas,i,j) +  gsl_matrix_get(betas,i,j) - sumProb));
		}
	}
	//cleanup
	gsl_matrix_free(alphas);
	gsl_matrix_free(betas);
	gsl_vector_free(tempVec);
}

/* posterior probs reduced- this fills a matrix of nstates * L observations with the
posterior probs as above except this time takes alphas and betas as args */

void posteriorProbsReduced(HMM *hmm, gsl_matrix *postProbMat, double pFor, gsl_matrix *alphas, gsl_matrix *betas, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
  gsl_vector *tempVec;
  int i, j;
  double  sumProb;

  //do the posterior probability thing- okay sir pull the switch
  tempVec = gsl_vector_alloc(hmm->nstates);
  for(j=0; j < length; j++){
    //first get sum for denominator
    for(i = 0; i < hmm->nstates; i++){
      gsl_vector_set(tempVec, i, (gsl_matrix_get(alphas,i,j) +  gsl_matrix_get(betas,i,j)));
    }
    sumProb = log_sum(tempVec);

    //check that sum looks like pFor
      //assert(fabs(pFor - sumProb) < 0.01);

    //then calculate marginal and set value (not log)
    for(i = 0; i < hmm->nstates; i++){
      gsl_matrix_set(postProbMat, i, j, exp(gsl_matrix_get(alphas,i,j) +  gsl_matrix_get(betas,i,j) - sumProb));
    }
  }
  //cleanup
   gsl_vector_free(tempVec);
}

/* posterior probs reduced log- this fills a matrix of nstates * L observations with the
posterior probs as above except this time takes alphas and betas as args */

void posteriorProbsReducedLog(HMM *hmm, gsl_matrix *postProbMat, double pFor, gsl_matrix *alphas, gsl_matrix *betas, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
  gsl_vector *tempVec;
  int i, j;
  double  sumProb;

  //do the posterior probability thing- okay sir pull the switch
  tempVec = gsl_vector_alloc(hmm->nstates);
  for(j=0; j < length; j++){
    //first get sum for denominator
    for(i = 0; i < hmm->nstates; i++){
      gsl_vector_set(tempVec, i, (gsl_matrix_get(alphas,i,j) +  gsl_matrix_get(betas,i,j)));
    }
    sumProb = log_sum(tempVec);

    //check that sum looks like pFor
    //assert(fabs(pFor - sumProb) < 0.01);

    //then calculate marginal and set value
    for(i = 0; i < hmm->nstates; i++){
      gsl_matrix_set(postProbMat, i, j, gsl_matrix_get(alphas,i,j) +  gsl_matrix_get(betas,i,j) - sumProb);
    }
  }
  //cleanup
   gsl_vector_free(tempVec);
}

/* meant to computer prob{state_i_t, state_j_t+1 | data, model} -- for restimating transitions, takes as an arg *jpMatArray an
array of gsl_matrix structs representing joint posterior probabilities -- log scale so need log posts */

void jointPosteriorProbsReduced(HMM *hmm, gsl_matrix **jpMatArray, double pFor, gsl_matrix *alphas, gsl_matrix *betas, gsl_matrix *emisLogs, int length, int transitionPowerFlag){

  int i, j, t;
  double  sumProb, val = 0;
  gsl_matrix *powTrans;

  //for each obs
  for(t=0; t < length - 1; t++){
    gsl_matrix_set_all(jpMatArray[t],0);
    //iterate i,j states
    for(i = 0; i < hmm->nstates; i++){
      for(j = 0; j < hmm->nstates; j++){
	//transition stuff
	if (transitionPowerFlag){

	  if (gsl_vector_get(hmm->otherData,t+1) -  gsl_vector_get(hmm->otherData, t) > 1){
	    powTrans = gsl_matrix_power_logs(hmm->transition_matrix, gsl_vector_get(hmm->otherData,t+1) \
					     -  gsl_vector_get(hmm->otherData, t));
	    val = gsl_matrix_get(alphas, i, t) + gsl_matrix_get(powTrans, i, j) \
	      + gsl_matrix_get(emisLogs, j, t+1) + gsl_matrix_get(betas, j, t+1) - pFor;
	    gsl_matrix_free(powTrans);
	  }
	  else{
	    val = gsl_matrix_get(alphas, i, t) + gsl_matrix_get(hmm->transition_score_matrix, i, j) \
	      + gsl_matrix_get(emisLogs, j, t+1) + gsl_matrix_get(betas, j, t+1) - pFor;
	  }
	}
	else{
	  val = gsl_matrix_get(alphas, i, t) + gsl_matrix_get(hmm->transition_score_matrix, i, j) \
		    + gsl_matrix_get(emisLogs, j, t+1) + gsl_matrix_get(betas, j, t+1) - pFor;
	}
	//set i,j value in gsl_matrix at t
	gsl_matrix_set(jpMatArray[t], i, j, val);
      }
    }
  }
}

/*restimateTransitions-- uses posteriors and joint posteriors to reestimate the transition matrix based on EM */
void reestimateTransitions(HMM *hmm, gsl_matrix *postProbMat, gsl_matrix **jpMatArray, double pFor, gsl_matrix *alphas, \
			   gsl_matrix *betas, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
  double sum_gamma, sum_cosi, val;
  int i, j, t,k;
  gsl_vector *tmpVec1, *tmpVec2;
  gsl_matrix *tempTrans, *powTrans;
  gsl_vector *rowSums;

  /* this is the joint posterior method- currently not operational */
  /*
  //calculate joints, no reason to do it twice...
  //  jointPosteriorProbsReduced(hmm, jpMatArray, pFor, alphas, betas, emisLogs, length ,transitionPowerFlag);
  //allocate tmpVecs
  tmpVec1 = gsl_vector_alloc(length - 2);
  tmpVec2 = gsl_vector_alloc(length - 2);
  //iterate over states
  for(i = 0; i < hmm->nstates; i++){
    for(j = 0; j < hmm->nstates; j++){
      gsl_vector_set_all(tmpVec1,0);
      gsl_vector_set_all(tmpVec2,0);
      //sum up numerator and denom
      for(t = 1; t < length - 1; t++){
	//	gsl_vector_set(tmpVec1, t - 1, gsl_matrix_get(postProbMat,i,t));
	//gsl_vector_set(tmpVec2, t - 1, gsl_matrix_get(jpMatArray[t],i,j));
	sum_gamma += gsl_matrix_get(postProbMat,i,t);
	sum_cosi += gsl_matrix_get(jpMatArray[t],i,j);
      }

      //gsl_matrix_set(hmm->transition_matrix, i, j, exp(log_sum(tmpVec1) - log_sum(tmpVec2)));
      // hmm_logify_transitions(hmm);
      gsl_matrix_set(hmm->transition_matrix, i, j, sum_cosi / sum_gamma);
       gsl_matrix_set(hmm->transition_score_matrix, i, j, log(sum_cosi / sum_gamma));
    }
  }
  */

  /* older method */
  tempTrans = gsl_matrix_alloc(hmm->nstates,hmm->nstates);
  rowSums = gsl_vector_alloc(hmm->nstates);

  gsl_matrix_set_all(tempTrans,0.0);
  gsl_vector_set_all(rowSums, 0.0);
  for(i = 1; i < length - 1; i++){
    for(j = 0; j < hmm->nstates; j++){
      for(k = 0; k < hmm->nstates; k++){
	if (transitionPowerFlag){
	  if (gsl_vector_get(hmm->otherData,i + 1) -  gsl_vector_get(hmm->otherData, i ) > 1){
	    powTrans = gsl_matrix_power_logs(hmm->transition_matrix, \
					     gsl_vector_get(hmm->otherData,i + 1) -  gsl_vector_get(hmm->otherData, i));
	    val = exp(gsl_matrix_get(alphas, j, i) + gsl_matrix_get(powTrans, j, k) \
		      + gsl_matrix_get(emisLogs, k, i+1) + gsl_matrix_get(betas, k, i+1) - pFor);
	    gsl_matrix_free(powTrans);
	  }
	  else{
	    val = exp(gsl_matrix_get(alphas, j, i) + gsl_matrix_get(hmm->transition_score_matrix, j, k) \
		      + gsl_matrix_get(emisLogs, k, i+1) + gsl_matrix_get(betas, k, i+1) - pFor);
	  }
	}
	else{
	    val = exp(gsl_matrix_get(alphas, j, i) + gsl_matrix_get(hmm->transition_score_matrix, j, k) \
		      + gsl_matrix_get(emisLogs, k, i+1) + gsl_matrix_get(betas, k, i+1) - pFor);
	}
      	gsl_matrix_set(tempTrans, j, k, gsl_matrix_get(tempTrans,j,k) + val);
	gsl_vector_set(rowSums, j, gsl_vector_get(rowSums, j) + val);
      }
    }
  }

  //update transition scores
  for(j = 0; j < hmm->nstates; j++){
    for(k = 0; k < hmm->nstates; k++){
      val = gsl_matrix_get(tempTrans, j, k) / gsl_vector_get(rowSums, j);
      gsl_matrix_set(hmm->transition_matrix, j, k, val);
      gsl_matrix_set(hmm->transition_score_matrix, j, k, log(val));
    }
  }
  gsl_matrix_free(tempTrans);
  gsl_vector_free(rowSums);
}

/*restimateTransitions_vanilla-- uses posteriors to reestimate the transition matrix based on EM */
void reestimateTransitions_vanilla(HMM *hmm, double pFor, gsl_matrix *alphas, \
gsl_matrix *betas, gsl_matrix *emisLogs, int length){
	double val;
	int i, j,k;
	gsl_vector *rowSums;
	gsl_matrix *tempTrans;

/* older method */
  	tempTrans = gsl_matrix_alloc(hmm->nstates,hmm->nstates);
	gsl_matrix_set_all(tempTrans,0.0);
	rowSums = gsl_vector_alloc(hmm->nstates);
	gsl_vector_set_all(rowSums, 0.0);
	for(i = 1; i < length - 1; i++){
		for(j = 0; j < hmm->nstates; j++){
			for(k = 0; k < hmm->nstates; k++){
				val = exp(gsl_matrix_get(alphas, j, i) + gsl_matrix_get(hmm->transition_score_matrix, j, k) \
					+ gsl_matrix_get(emisLogs, k, i+1) + gsl_matrix_get(betas, k, i+1) - pFor);

				gsl_matrix_set(tempTrans, j, k, gsl_matrix_get(tempTrans,j,k) + val);
				gsl_vector_set(rowSums, j, gsl_vector_get(rowSums, j) + val);
			}
		}
	}

//update transition scores
	for(j = 0; j < hmm->nstates; j++){
		for(k = 0; k < hmm->nstates; k++){
			val = gsl_matrix_get(tempTrans, j, k) / gsl_vector_get(rowSums, j);
			gsl_matrix_set(hmm->transition_matrix, j, k, val);
			gsl_matrix_set(hmm->transition_score_matrix, j, k, log(val));
		}
	}
	gsl_matrix_free(tempTrans);
	gsl_vector_free(rowSums);
}

/* posteriorDecode-- fills a vector (posteriorPath) of length L observations
with the most probably state sequence based on posterior probabilities */
void posteriorDecode(HMM *hmm, gsl_vector *posteriorPath, gsl_matrix *posts, int length){
	int i, j, maxState;
	double maxProb;

	assert(length > 0 && hmm != NULL);
	assert(posteriorPath->size == length);
	assert(posts->size1 == hmm->nstates && posts->size2 == length);

	for(i=0; i < length; i++){
		maxProb = 0;
		maxState = 0;
		for(j=0;j < hmm->nstates;j++){
			if (gsl_matrix_get(posts, j, i) > maxProb){
				maxProb = gsl_matrix_get(posts, j, i);
				maxState = j;
			}
		}
		gsl_vector_set(posteriorPath,i,maxState);
	}

}

/* viterbi algorithm- this fills a vector (viterbiPath) of length L observations with
the most probable state sequence. it returns the prob of the most probable path.
a matrix of emission probs (log!!!) of dimensions nstates * L observations must be supplied. */

  double viterbi(HMM *hmm, gsl_vector *viterbiPath, gsl_matrix *emisLogs, int length, int transitionPowerFlag){
  gsl_matrix *deltas, *psi, *logTransitions;
	gsl_vector_view col;
	int i, j, lastState, tmp, last, power;
	double tempProb,prob, emit;
	int point;

	assert(length > 0 && hmm != NULL);
	assert(viterbiPath->size == length);
	assert(emisLogs->size1 == hmm->nstates && emisLogs->size2 == length);

	//if using transition powers, then check that we have other data
	if (transitionPowerFlag){
	  assert(hmm->otherData != NULL);
	  //initialize last
	  last = gsl_vector_get(hmm->otherData,0);
	}
	else {
	    last = 0; /* not used */
	}

	//allocate matrices for deltas (log prob of most prob path to stateN,obsL) and psis (the backpointers)
	deltas = gsl_matrix_alloc(hmm->nstates, length);
	psi    = gsl_matrix_alloc(hmm->nstates, length);

	//initialization
	for (i = 0; i < hmm->nstates; i++){
		prob = gsl_vector_get(hmm->piStart_scores, i);
		emit = gsl_matrix_get(emisLogs, i, 0);
		gsl_matrix_set(deltas, i, 0, (prob + emit));
		gsl_matrix_set(psi, i, 0, 0);
		}

	point = 666;
	//recursion
	for (j = 1; j < length; j++){
	  for(i = 0; i < hmm->nstates; i++){
	    /* transition powers? */
	    if (transitionPowerFlag){
	      power = gsl_vector_get(hmm->otherData,j) - last;
	      logTransitions = gsl_matrix_power_logs(hmm->transition_matrix, power);
	      tempProb = viterbiMaxPrevious(hmm, deltas, logTransitions, i, j, &point);
	      gsl_matrix_free(logTransitions);
	    }
	    else{
	      tempProb =  viterbiMaxPrevious(hmm, deltas, hmm->transition_matrix, i, j, &point);
	    }
	    gsl_matrix_set(deltas, i, j, tempProb + gsl_matrix_get(emisLogs, i, j));
	    gsl_matrix_set(psi, i ,j , point);
	  }
	  if (transitionPowerFlag){
	      last = gsl_vector_get(hmm->otherData,j);
	  }
	}

	//termination
	col = gsl_matrix_column(deltas, length - 1);
	lastState = (int) gsl_vector_max_index(&col.vector);

	//backtrack
	gsl_vector_set(viterbiPath, length - 1, lastState);
	for (j = length - 2; j >= 0; j--){
		tmp = gsl_matrix_get(psi, lastState, j + 1);
		lastState =  tmp;
		gsl_vector_set(viterbiPath, j , (int) lastState);
		}

	return gsl_vector_max(&col.vector);
	gsl_matrix_free(deltas);
	gsl_matrix_free(psi);
}

/* this returns the sum of the previous states for the forward probabilities */

double forwardSumPrevious(HMM *hmm, gsl_matrix *alphas, gsl_matrix *transition_score_matrix_i, int state, int obsIndex){
	gsl_vector *tempProbs;
	gsl_vector_view prevCol;
	int i;
	double tempProb;

	tempProbs = gsl_vector_alloc(hmm->nstates);
	prevCol = gsl_matrix_column(alphas, obsIndex - 1);
	for(i = 0; i < hmm->nstates; i++){
		tempProb = gsl_vector_get(&prevCol.vector, i) + gsl_matrix_get(transition_score_matrix_i, i, state);
		gsl_vector_set(tempProbs, i, tempProb);
		}
	tempProb = log_sum(tempProbs);
	gsl_vector_free(tempProbs);
	return(tempProb);
}

/* this returns the sum of the "next" states for the backward probabilities */

double backwardSumNext(HMM *hmm, gsl_matrix *betas, gsl_matrix *transition_score_matrix_j,gsl_matrix *emisLogs, int state, int obsIndex){
	gsl_vector *tempProbs;
	gsl_vector_view nextCol;
	int j;
	double tempProb;

	tempProbs = gsl_vector_alloc(hmm->nstates);
	nextCol = gsl_matrix_column(betas, obsIndex + 1);
	for(j = 0; j < hmm->nstates; j++){		//sum over begin states...
		tempProb = gsl_vector_get(&nextCol.vector, j) + gsl_matrix_get(transition_score_matrix_j, state, j) + gsl_matrix_get(emisLogs, j,obsIndex + 1) ;
		gsl_vector_set(tempProbs, j, tempProb);
		}
	tempProb = log_sum(tempProbs);
	gsl_vector_free(tempProbs);
	return(tempProb);
}

/* this returns the max of the previous states for the viterbi algorithm.
it also sets the back pointer (point) to that state. */

double viterbiMaxPrevious(HMM *hmm, gsl_matrix *deltas, gsl_matrix *transition_score_matrix_i, int state, int obsIndex, int *point){
	gsl_vector *tempProbs;
	gsl_vector_view prevCol;
	int i;
	double tempProb;

	tempProbs = gsl_vector_alloc(hmm->nstates);
	prevCol = gsl_matrix_column(deltas, obsIndex - 1);
	for(i = 0; i < hmm->nstates; i++){
		tempProb = gsl_vector_get(&prevCol.vector, i) + gsl_matrix_get(transition_score_matrix_i, i, state);
		gsl_vector_set(tempProbs, i, tempProb);
		}
	*point = gsl_vector_max_index(tempProbs);
	tempProb = gsl_vector_max(tempProbs);
	gsl_vector_free(tempProbs);
	return(tempProb);
}

/* This computes the log likelihood of an subpath of the data, for a specified set of the states defined
    in the vector stateInclusion (1 for include, 0 for exclude). */
double hmm_subpath_score(HMM *hmm, gsl_matrix *emisLogs, gsl_vector *stateInclusion, int begin, int length, int transitionPowerFlag){
  gsl_matrix *alphas, *real_trans ;
  gsl_vector *real_begin;
  int i, j, stateCount;
  double score;

  //initialize some matrices
  alphas = gsl_matrix_alloc(hmm->nstates, length);
  real_trans = gsl_matrix_alloc(hmm->nstates, hmm->nstates);
  real_begin = gsl_vector_alloc(hmm->nstates);

  //count number of states under consideration
  stateCount = 0;
  for(i = 0; i < hmm->nstates; i++){
    stateCount += gsl_vector_get(stateInclusion, i);
  }

  //keep track of original transition matrix and "begin" vector
  gsl_matrix_memcpy(real_trans,hmm->transition_matrix);
  gsl_vector_memcpy(real_begin, hmm->piStart);

  //set begin states to uniform distribution over states considered in subset
  for(i = 0; i < hmm->nstates; i++){
    if (gsl_vector_get(stateInclusion,i)){
      gsl_vector_set(hmm->piStart, i, 1.0 / stateCount);
    }
    else{
      gsl_vector_set(hmm->piStart, i, 0.0);
    }
  }

  //renomalize transition matrix to reflect states included.
  //go through transition matrix, shutting off those which aren't in subset
  for(i = 0; i < hmm->nstates; i++){
    for(j = 0; j < hmm->nstates; j++){
      if (gsl_vector_get(stateInclusion,i) == 0 || gsl_vector_get(stateInclusion,j) == 0){
	gsl_matrix_set( hmm->transition_matrix, i, j, 0.0);
      }
    }
  }

  //one is the magic number...
  hmm_normalize_transitions(hmm);
  hmm_logify_transitions(hmm);

  //run the forward algorithm to get our answer!
  score = forwardAlgOffset(hmm, alphas, emisLogs, length, begin, transitionPowerFlag);

  //cleanup
  gsl_matrix_memcpy(hmm->transition_matrix, real_trans);
  gsl_vector_memcpy(hmm->piStart, real_begin);
  hmm_logify_transitions(hmm);
  gsl_matrix_free(alphas);
  gsl_matrix_free(real_trans);
  gsl_vector_free(real_begin);
  return(score);
}

/* Calculates the log odds score for two competing sets of states, over a subset of the data
   by comparing the ratio of their likelihoods */
double hmm_subpath_log_odds(HMM *hmm, gsl_matrix *emisLogs, gsl_vector *stateInclusion1,
			     gsl_vector *stateInclusion2,int begin, int length,
			    int transitionPowerFlag){
  double score1, score2;
  score1 = hmm_subpath_score(hmm, emisLogs, stateInclusion1, begin,length,transitionPowerFlag);
  score2 =  hmm_subpath_score(hmm, emisLogs, stateInclusion2, begin,length,transitionPowerFlag);
  //printf("scores: %f %f\n",score1,score2);
  return(score1 - score2);
}

/* simulatePath-- returns a vector snpNumber long that represents an instance
of the simulated markov state path through the data. takes as arguments,an hmm,
 and a pointer to void which actually is an gsl random numb. gen, as well as the observation
 number */
gsl_vector *simulatePath(HMM *h, void *r, int obsNumber){
  int i, j, pastState;
  double sum, rand;
  gsl_rng * rn = (gsl_rng *) r;
  gsl_vector *path;

  path = gsl_vector_alloc(obsNumber);

  /*choose initial state, set paths[0] */
  rand = gsl_rng_uniform(rn);
  sum = 0;
  for(i = 0; i < h->nstates; i++){
    sum += gsl_vector_get(h->piStart, i);
    printf("%f %f\n",rand, sum);
    if (rand <= sum){
      printf("true\n");
      gsl_vector_set(path,0,i);
      rand = 2.0;
    }
  }

  /* now go through obs, using transition matrix to choose states */

  for(i = 1; i < obsNumber; i++){
    pastState = gsl_vector_get(path, i-1);
    rand = gsl_rng_uniform(rn);
    sum = 0;
    for(j = 0; j < h->nstates; j++){
      sum += gsl_matrix_get(h->transition_matrix, pastState, j);
      if (rand <= sum){
	gsl_vector_set(path,i,j);
	rand = 2.0;
      }
    }
  }
  return(path);
}

/* simulatePathSpaced-- just like simulatePath except takes two
added parameters, a pointer to a gsl_vector which will contain the
simulated snp locations, and a double for the exponential mean */
gsl_vector *simulatePathSpaced(HMM *h, void *r, int obsNumber, gsl_vector *locs, double mu){
  int i, j, pastState;
  double sum, rand;
  gsl_rng * rn = (gsl_rng *) r;
  gsl_vector *path;
  gsl_matrix *matPower;

  path = gsl_vector_alloc(obsNumber);

  /*choose initial state, set paths[0] */
  rand = gsl_rng_uniform(rn);
  sum = 0;
  for(i = 0; i < h->nstates; i++){
    sum += gsl_vector_get(h->piStart, i);
    printf("%f %f\n",rand, sum);
    if (rand <= sum){
      printf("true\n");
      gsl_vector_set(path,0,i);
      rand = 2.0;
    }
  }

  /*set initial location to 1 by convention */
  gsl_vector_set(locs,0,1);
  //now go through obs and simulate spacing -- curently drawn from exp
  for(i = 1; i < obsNumber; i++){
    rand = gsl_ran_exponential(rn, mu);
    gsl_vector_set(locs, i, gsl_vector_get(locs, i - 1) + ceil(rand));
  }

  /* now go through obs, using transition matrix and powers to choose states */
  for(i = 1; i < obsNumber; i++){
    pastState = gsl_vector_get(path, i-1);
    matPower = gsl_matrix_power(h->transition_matrix, gsl_vector_get(locs,i) - gsl_vector_get(locs,i - 1));
    rand = gsl_rng_uniform(rn);
    sum = 0;
    for(j = 0; j < h->nstates; j++){
      sum += gsl_matrix_get(matPower, pastState, j);
      if (rand <= sum){
	gsl_vector_set(path,i,j);
	rand = 2.0;
      }
    }
    gsl_matrix_free(matPower);
  }
  return(path);
}