/* sys/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_test.h>

#include <gsl/gsl_ieee_utils.h>

int
main (void)
{
  double y, y_expected;

  gsl_ieee_env_setup ();

  /* Test for expm1 */

  y = gsl_expm1 (0.0); y_expected = 0.0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(0.0)");

  y = gsl_expm1 (1e-10); y_expected = 1.000000000050000000002e-10;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(1e-10)");

  y = gsl_expm1 (-1e-10); y_expected = -9.999999999500000000017e-11;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(-1e-10)");

  y = gsl_expm1 (0.1); y_expected = 0.1051709180756476248117078264902;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(0.1)");

  y = gsl_expm1 (-0.1); y_expected = -0.09516258196404042683575094055356;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(-0.1)");

  y = gsl_expm1 (10.0); y_expected = 22025.465794806716516957900645284;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(10.0)");

  y = gsl_expm1 (-10.0); y_expected = -0.99995460007023751514846440848444;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_expm1(-10.0)");

  /* Test for log1p */

  y = gsl_log1p (0.0); y_expected = 0.0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_log1p(0.0)");

  y = gsl_log1p (1e-10); y_expected = 9.9999999995000000000333333333308e-11;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_log1p(1e-10)");

  y = gsl_log1p (0.1); y_expected = 0.095310179804324860043952123280765;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_log1p(0.1)");

  y = gsl_log1p (10.0); y_expected = 2.3978952727983705440619435779651;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_log1p(10.0)");

  /* Test for gsl_hypot */

  y = gsl_hypot (0.0, 0.0) ; y_expected = 0.0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(0.0, 0.0)");

  y = gsl_hypot (1e-10, 1e-10) ; y_expected = 1.414213562373095048801688e-10;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(1e-10, 1e-10)");

  y = gsl_hypot (1e-38, 1e-38) ; y_expected = 1.414213562373095048801688e-38;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(1e-38, 1e-38)");

  y = gsl_hypot (1e-10, -1.0) ; y_expected = 1.000000000000000000005;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(1e-10, -1)");

  y = gsl_hypot (-1.0, 1e-10) ; y_expected = 1.000000000000000000005;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(-1, 1e-10)");

  y = gsl_hypot (1e307, 1e301) ; y_expected = 1.000000000000499999999999e307;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(1e307, 1e301)");

  y = gsl_hypot (1e301, 1e307) ; y_expected = 1.000000000000499999999999e307;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(1e301, 1e307)");

  y = gsl_hypot (1e307, 1e307) ; y_expected = 1.414213562373095048801688e307;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_hypot(1e307, 1e307)");

  /* Test for acosh */

  y = gsl_acosh (1.0); y_expected = 0.0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_acosh(1.0)");

  y = gsl_acosh (1.1); y_expected = 4.435682543851151891329110663525e-1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_acosh(1.1)");

  y = gsl_acosh (10.0); y_expected = 2.9932228461263808979126677137742e0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_acosh(10.0)");

  y = gsl_acosh (1e10); y_expected = 2.3718998110500402149594646668302e1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_acosh(1e10)");

  /* Test for asinh */

  y = gsl_asinh (0.0); y_expected = 0.0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(0.0)");

  y = gsl_asinh (1e-10); y_expected = 9.9999999999999999999833333333346e-11;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(1e-10)");

  y = gsl_asinh (-1e-10); y_expected = -9.9999999999999999999833333333346e-11;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(1e-10)");

  y = gsl_asinh (0.1); y_expected = 9.983407889920756332730312470477e-2;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(0.1)");

  y = gsl_asinh (-0.1); y_expected = -9.983407889920756332730312470477e-2;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(-0.1)");

  y = gsl_asinh (1.0); y_expected = 8.8137358701954302523260932497979e-1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(1.0)");

  y = gsl_asinh (-1.0); y_expected = -8.8137358701954302523260932497979e-1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(-1.0)");

  y = gsl_asinh (10.0); y_expected = 2.9982229502979697388465955375965e0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(10)");

  y = gsl_asinh (-10.0); y_expected = -2.9982229502979697388465955375965e0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(-10)");

  y = gsl_asinh (1e10); y_expected = 2.3718998110500402149599646668302e1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(1e10)");

  y = gsl_asinh (-1e10); y_expected = -2.3718998110500402149599646668302e1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_asinh(-1e10)");

  /* Test for atanh */

  y = gsl_atanh (0.0); y_expected = 0.0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(0.0)");

  y = gsl_atanh (1e-20); y_expected = 1e-20;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(1e-20)");

  y = gsl_atanh (-1e-20); y_expected = -1e-20;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(-1e-20)");

  y = gsl_atanh (0.1); y_expected = 1.0033534773107558063572655206004e-1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(0.1)");

  y = gsl_atanh (-0.1); y_expected = -1.0033534773107558063572655206004e-1;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(-0.1)");

  y = gsl_atanh (0.9); y_expected = 1.4722194895832202300045137159439e0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(0.9)");

  y = gsl_atanh (-0.9); y_expected = -1.4722194895832202300045137159439e0;
  gsl_test_rel (y, y_expected, 1e-15, "gsl_atanh(0.9)");

  /* Test for pow_int */

  y = gsl_pow_2 (-3.14); y_expected = pow(-3.14, 2.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_2(-3.14)");

  y = gsl_pow_3 (-3.14); y_expected = pow(-3.14, 3.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_3(-3.14)");

  y = gsl_pow_4 (-3.14); y_expected = pow(-3.14, 4.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_4(-3.14)");

  y = gsl_pow_5 (-3.14); y_expected = pow(-3.14, 5.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_5(-3.14)");

  y = gsl_pow_6 (-3.14); y_expected = pow(-3.14, 6.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_6(-3.14)");

  y = gsl_pow_7 (-3.14); y_expected = pow(-3.14, 7.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_7(-3.14)");

  y = gsl_pow_8 (-3.14); y_expected = pow(-3.14, 8.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_8(-3.14)");

  y = gsl_pow_9 (-3.14); y_expected = pow(-3.14, 9.0);
  gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_9(-3.14)");

  {
    int n;
    for (n = -9; n < 10; n++) {
      y = gsl_pow_int (-3.14, n); y_expected = pow(-3.14, n);
      gsl_test_rel (y, y_expected, 1e-15, "gsl_pow_n(-3.14,%d)", n);
    }
  }

  /* Test for isinf, isnan, finite*/

  {
    double zero, one, inf, nan;
    int s;

    zero = 0.0;
    one = 1.0;
    inf = exp(1.0e10);
    nan = inf / inf;

    s = gsl_isinf(zero);
    gsl_test_int (s, 0, "gsl_isinf(0)");

    s = gsl_isinf(one);
    gsl_test_int (s, 0, "gsl_isinf(1)");

    s = gsl_isinf(inf);
    gsl_test_int (s, 1, "gsl_isinf(inf)");

    s = gsl_isinf(-inf);
    gsl_test_int (s, -1, "gsl_isinf(-inf)");

    s = gsl_isinf(nan);
    gsl_test_int (s, 0, "gsl_isinf(nan)");

    s = gsl_isnan(zero);
    gsl_test_int (s, 0, "gsl_isnan(0)");

    s = gsl_isnan(one);
    gsl_test_int (s, 0, "gsl_isnan(1)");

    s = gsl_isnan(inf);
    gsl_test_int (s, 0, "gsl_isnan(inf)");

    s = gsl_isnan(nan);
    gsl_test_int (s, 1, "gsl_isnan(nan)");

    s = gsl_finite(zero);
    gsl_test_int (s, 1, "gsl_finite(0)");

    s = gsl_finite(one);
    gsl_test_int (s, 1, "gsl_finite(1)");

    s = gsl_finite(inf);
    gsl_test_int (s, 0, "gsl_finite(inf)");

    s = gsl_finite(nan);
    gsl_test_int (s, 0, "gsl_finite(nan)");
  }

  {
    double x = gsl_fdiv (2.0, 3.0);
    gsl_test_rel (x, 2.0/3.0, 4*GSL_DBL_EPSILON, "gsl_fdiv(2,3)");
  }

  exit (gsl_test_summary ());
}


#ifndef Single_Pin_Subchannel_h
#define Single_Pin_Subchannel_h

#include <vector>

// Trilinos includes
#include "Teuchos_ParameterList.hpp"
#include "Teuchos_RCP.hpp"

// SCALE includes
#include "Nemesis/harness/DBC.hh"

// vendored includes
#include <gsl/gsl>

namespace enrico {

//===========================================================================//
/*!
 * \class Single_Pin_Subchannel
 * \brief Solve subchannel flow in single channel.
 *
 * This class implements a two-equation subchannel model involving
 * conservation of energy and axial momentum.  No lateral flow is accounted
 * for.  The model excludes friction momentum losses and any spacer grid
 * effects.  It is useful for giving qualitatively-correct behavior but
 * should not be used for actual analysis.
 */
//===========================================================================//
class Single_Pin_Subchannel {
public:
  //@{
  //! Typedefs
  using RCP_PL = Teuchos::RCP<Teuchos::ParameterList>;
  //@}

  enum Verbosity { NONE, LOW, HIGH };

private:
  // >>> DATA

  // Solve parameters
  double d_tol;
  int d_max_iters;
  Verbosity d_verbosity;

  // Axial grid
  std::vector<double> d_delta_z;

  // Channel geometry
  double d_area;

  // Flow conditions
  double d_mdot;
  double d_T_inlet;
  double d_p_exit;

public:
  // Constructor
  Single_Pin_Subchannel(RCP_PL& parameters, const std::vector<double>& delta_z);

  // Set channel area (cm^2)
  void set_channel_area(double area)
  {
    // Channel area internally is needed in m^2
    Expects(area > 0.0);
    d_area = 1e-4 * area;
  }

  // Set mass flow rate (kg/s)
  void set_mass_flow_rate(double mdot)
  {
    Expects(mdot > 0.0);
    Expects(mdot < 2.0);
    d_mdot = mdot;
  }

  // Set inlet temperature (K)
  void set_inlet_temperature(double T)
  {
    Expects(T > 0);
    Expects(T < 1000);
    d_T_inlet = T;
  }

  // Set exit pressure (Pa)
  void set_exit_pressure(double p)
  {
    Expects(p > 0);
    Expects(p < 2.2e7);
    d_p_exit = p;
  }

  // Solve for single subchannel given power distribution
  void solve(const std::vector<double>& power,
             std::vector<double>& temperature,
             std::vector<double>& density);
};

//---------------------------------------------------------------------------//
} // end namespace enrico

//---------------------------------------------------------------------------//
#endif // Single_Pin_Subchannel_h

//---------------------------------------------------------------------------//
// end of Single_Pin_Subchannel.h
//---------------------------------------------------------------------------//


#pragma once

#include <gsl/gsl>
#include <memory>
#include <tuple>

#include "../math/Constants.h"
#include "../math/Exponentials.h"
#include "../math/General.h"
#include "../math/Point2.h"
#include "../math/Point3.h"
#include "../math/Random.h"
#include "../math/TrigFuncs.h"
#include "../math/Vec2.h"
#include "../math/Vec3.h"
#include "../utils/Concepts.h"
/// @brief custom mathematics implementaitons
namespace math { } // namespace math

/// @brief utility types
namespace utils { } // namespace utils

using utils::concepts::ConstructibleFrom;		// NOLINT
using utils::concepts::Copyable;				// NOLINT
using utils::concepts::CopyOrMovable;			// NOLINT
using utils::concepts::DefaultConstructible;	// NOLINT
using utils::concepts::Derived;					// NOLINT
using utils::concepts::FloatingPoint;			// NOLINT
using utils::concepts::Integral;				// NOLINT
using utils::concepts::Movable;					// NOLINT
using utils::concepts::NotCopyable;				// NOLINT
using utils::concepts::NotCopyOrMovable;		// NOLINT
using utils::concepts::NotDefaultConstructible; // NOLINT
using utils::concepts::NotMovable;				// NOLINT
using utils::concepts::NotPassable;				// NOLINT
using utils::concepts::NotPointer;				// NOLINT
using utils::concepts::NotReference;			// NOLINT
using utils::concepts::NotSemiRegular;			// NOLINT
using utils::concepts::Numeric;					// NOLINT
using utils::concepts::Passable;				// NOLINT
using utils::concepts::Pointer;					// NOLINT
using utils::concepts::Reference;				// NOLINT
using utils::concepts::SemiRegular;				// NOLINT
using utils::concepts::SignedIntegral;			// NOLINT
using utils::concepts::SignedNumeric;			// NOLINT

using math::Constants;	  // NOLINT
using math::Exponentials; // NOLINT
using math::General;	  // NOLINT
using math::Point2;		  // NOLINT
using math::Point2Idx;	  // NOLINT
using math::Point3;		  // NOLINT
using math::Point3Idx;	  // NOLINT
using math::random_value; // NOLINT
using math::Trig;		  // NOLINT
using math::Vec2;		  // NOLINT
using math::Vec2Idx;	  // NOLINT
using math::Vec3;		  // NOLINT
using math::Vec3Idx;	  // NOLINT

using gsl::narrow_cast; // NOLINT
template<typename T>
using NotNull = gsl::not_null<T*>; // NOLINT
template<typename T>
using Owner = gsl::owner<T*>; // NOLINT

template<FloatingPoint T = float>
inline constexpr auto degrees_to_radians(T degrees) noexcept -> T {
	return degrees * Constants<T>::pi / narrow_cast<T>(180.0);
}

template<FloatingPoint T = float>
inline constexpr auto radians_to_degrees(T radians) noexcept -> T {
	return radians * narrow_cast<T>(180.0) / Constants<T>::pi;
}

template<SignedNumeric T = float>
inline constexpr auto clamp(T val, T min, T max) noexcept -> T {
	if(val < min) {
		return min;
	}
	else if(val > max) {
		return max;
	}
	else {
		return val;
	}
}

template<typename... Args>
inline constexpr auto ignore(Args&&... args) noexcept -> void {
	std::ignore = std::tuple<Args...>(args...);
}

// clang-format off
#ifndef _MSC_VER
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_MACROS_START \
		_Pragma("GCC diagnostic push")\
		_Pragma("GCC diagnostic ignored \"-Wunused-macros\"")
#else
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_MACROS_START
#endif
// clang-format on

// clang-format off
#ifndef _MSC_VER
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_MACROS_STOP \
		_Pragma("GCC diagnostic pop")
#else
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_MACROS_STOP
#endif
// clang-format on

IGNORE_UNUSED_MACROS_START

// clang-format off
#ifndef _MSC_VER
	// NOLINTNEXTLINE
	#define IGNORE_PADDING_START \
		_Pragma("GCC diagnostic push") \
		_Pragma("GCC diagnostic ignored \"-Wpadded\"")
#else
	// NOLINTNEXTLINE
	#define IGNORE_PADDING_START \
		_Pragma("warning( push )") \
		_Pragma("warning( disable : 4820 )")
#endif
// clang-format on

// clang-format off
#ifndef _MSC_VER
	// NOLINTNEXTLINE
	#define IGNORE_PADDING_STOP \
		_Pragma("GCC diagnostic pop")
#else
	// NOLINTNEXTLINE
	#define IGNORE_PADDING_STOP \
		_Pragma("warning( pop )")
#endif
// clang-format on

// clang-format off
#ifndef _MSC_VER
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_TEMPLATES_START \
		_Pragma("GCC diagnostic push")\
		_Pragma("GCC diagnostic ignored \"-Wunused-template\"")
#else
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_TEMPLATES_START
#endif
// clang-format on

// clang-format off
#ifndef _MSC_VER
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_TEMPLATES_STOP \
		_Pragma("GCC diagnostic pop")
#else
	// NOLINTNEXTLINE
	#define IGNORE_UNUSED_TEMPLATES_STOP
#endif
// clang-format on

IGNORE_UNUSED_MACROS_STOP


/*
** 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);
}


/** @file */

#ifndef __CCL_F1D_H_INCLUDED__
#define __CCL_F1D_H_INCLUDED__

#include <gsl/gsl_spline.h>

CCL_BEGIN_DECLS

typedef enum ccl_f1d_extrap_t {
  ccl_f1d_extrap_0 = 0,  // No extrapolation
  ccl_f1d_extrap_const = 410,  // Constant extrapolation
  ccl_f1d_extrap_linx_liny = 411,  // Linear x, linear y
  ccl_f1d_extrap_linx_logy = 412,  // Linear x, log y
  ccl_f1d_extrap_logx_liny = 413,  // Log x, linear y
  ccl_f1d_extrap_logx_logy = 414,  // Log x, log y
} ccl_f1d_extrap_t;

/*
 * Spline wrapper
 * Used to take care of evaluations outside the supported range
 */
typedef struct {
  gsl_spline *spline;
  double y0,yf; //Constant values to use beyond interpolation limit
  ccl_f1d_extrap_t extrap_lo_type;
  ccl_f1d_extrap_t extrap_hi_type;
  double x_ini, x_end; //Interpolation limits
  double y_ini, y_end;
  double der_lo;
  double der_hi;
} ccl_f1d_t;

ccl_f1d_t *ccl_f1d_t_new(int n,double *x,double *y,double y0,double yf,
			 ccl_f1d_extrap_t extrap_lo_type,
			 ccl_f1d_extrap_t extrap_hi_type, int *status);

double ccl_f1d_t_eval(ccl_f1d_t *spl,double x);

void ccl_f1d_t_free(ccl_f1d_t *spl);

CCL_END_DECLS

#endif


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


#ifndef __bvd_rng_h_
#define __bvd_rng_h_

#include "Cow.h"
#include <cmath>
#include <gsl/gsl_rng.h>
#include <time.h>
#include "Model_Constants.h"
#include <cmath>
#include <gsl/gsl_rng.h>
#include <time.h>

class Random_Number_Generator
{
 public:
	Random_Number_Generator();
	Random_Number_Generator( unsigned int seed );
	void init( unsigned int seed);
	/// draw a random integer/double between lo (inclusive) and hi (exclusive)
	/// from a uniform distribution.
	int          ran_unif_int(    int hi   , int lo=0);
	double       ran_unif_double( double hi, double lo=0 );

	int ran_triangular_int(double lo,double hi,double mod);
	double ran_triangular_double(double lo,double hi,double mod);

	int ran_poisson_int(int r, double mu);

	// true or false, depending on whether a calf ( age < bvd_const::age_threshold_calf ) just infected with BVD will die or not
	bool         will_TI_calf_die();
	double       time_of_death_infected_calf();
	double       time_of_next_infection(double rate);
	Calf_Status calf_outcome_from_infection      ( double  time_of_pregnancy );
	double       time_of_abortion_due_to_infection( double  time_of_pregnancy );
	double       time_of_rest_after_calving(int calving_number);

	// true or false if a birth is a deadbirth, assuming that the infection status of neither mother nor calf are known.
	// This depends on the age of the mother, as indicated in table 4.1 of the handout ("Anteil nicht lebend ausgetragener Kälber, Färsen/Kühle")
	// The wording of the handout is not clear whether in the figures of the handout also abortions are included.
	// In this function, abortions are *not* included.
	bool         is_this_a_deadbirth( bool first_pregnancy );

	//How long is a calf protected by the maternal antibodies in the colostrum.
	double       duration_of_MA();

	//Duration of an infection with BVD ( time that the infection state 'transiently infected' lasts )
	double       duration_of_infection();
	double         duration_of_pregnancy();
	double staggering_first_inseminations();
	int          number_of_calvings();
	virtual bool         is_calf_female();
	//  double         lifetime_male_calves();
	double       first_insemination_age();
	double       lifetime_PI();
	double       time_of_death_as_calf();
	double       insemination_result( bool first_pregnancy , bool* conception );
	double       life_expectancy_male_cow();

	//It has to be decided whether a beginning pregnancy will lead to an abortion or a birth.
	// Also here, pointer *birth is meant for output. if false, abortion is the case. If true birth is the case.
	// The return value is the time until the event (in case of birth -> duration of pregnancy, in case of abortion -> time until abortion ).
	// afaik, in the statistics we have, only the total abortion rates are given.
	// We don't know yet (as of 10.12.2015) whether there is a correlation between abortion happening and the infection status of the mother or the age of the mother.
	// So for now, treat it as uncorrelated. And use the total abortion rates given in the handout.
	double       conception_result( double age_of_mother, Infection_Status is_of_mother, bool* birth );

	void getNRandomNumbersInRange(int n, int rangeLeft, int rangeRight, int* resultArray);
	//double cowWellTimeOfBirth(double time);
	bool bloodTestRightResult();

	double removeTimeAfterFirstTest();
	double removeTimeAfterSecondTest();
	double retestTime();
	unsigned int getSeed();
	bool 		 cowGetsASecondChance();
	double 		timeOfFirstTest();
	virtual bool 		vaccinationWorks();
 private:
  gsl_rng * generator;
unsigned int seed ;
};

#endif


// 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);
}