/*
Deterministic Bayesian Sparse Linear Mixed Model (DBSLMM)
Copyright (C) 2019  Sheng Yang and Xiang Zhou

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, see <http://www.gnu.org/licenses/>.
*/

#include <vector>
#include <string>
#include <iostream>
#include <ctime>
#include <math.h>

#include <armadillo>

#include "../include/dtpr.hpp"
#include "../include/dbslmm.hpp"
#include "../include/tobool.hpp"

using namespace std;
using namespace arma;

DBSLMM::DBSLMM(void) :
	version("0.3"), date("05/01/2021"), year("2021")
{}

void DBSLMM::printHeader(void)
{
	cout << endl;
	cout << "*************************************************************"<< endl;
	cout << "  Deterministic Bayesian Sparse Linear Mixed Model (DBSLMM)  " << endl;
	cout << "  Version " << version << ", " << date << "                  " << endl;
	cout << "  Visit http://www.xzlab.org/software.html For Update        " << endl;
	cout << "  (C) " << year << " Sheng Yang, Xiang Zhou                  " << endl;
	cout << "  GNU General Public License                                 " << endl;
	cout << "  For Help, Type ./dbslmm -h                                 " << endl;
	cout << "*************************************************************" << endl;
	cout << endl;

	return;
}

void DBSLMM::printHelp(void) {
	cout << " FILE I/O RELATED OPTIONS" << endl;
	cout << " -s        [filename]  " << " specify input the summary data for the small effect SNPs." << endl;
	cout << " -l        [filename]  " << " specify input the summary data for the large effect SNPs." << endl;
	cout << " -r        [filename]  " << " specify input the bfile of reference data." << endl;
	cout << " -n        [num]       " << " specify input the sample size of the summary data." << endl;
	cout << " -mafMax   [num]       " << " specify input the maximium of the difference between reference panel and summary data." << endl;
	cout << " -nsnp     [num]  " << " specify input the number of snp." << endl;
	cout << " -b        [num]       " << " specify input the block information." << endl;
	cout << " -h        [num]       " << " specify input the heritability." << endl;
	cout << " -t        [filename]  " << " specify input thread." << endl;
	cout << " -eff      [filename]  " << " specify output the estimate effect SNPs." << endl;
	return;
}

void DBSLMM::Assign(int argc, char ** argv, PARAM &cPar) {

	string str;
	for (int i = 0; i < argc; i++) {

		if (strcmp(argv[i], "--smallEff") == 0 || strcmp(argv[i], "-s") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.s = str;
		}
		else if (strcmp(argv[i], "--largeEff") == 0 || strcmp(argv[i], "-l") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.l = str;
		}
		else if (strcmp(argv[i], "--reference") == 0 || strcmp(argv[i], "-r") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.r = str;
		}
		else if (strcmp(argv[i], "--N") == 0 || strcmp(argv[i], "-n") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.n = atoi(str.c_str());
		}
		else if (strcmp(argv[i], "--mafMax") == 0 || strcmp(argv[i], "-mafMax") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.mafMax = atof(str.c_str());
		}
		else if (strcmp(argv[i], "--numSNP") == 0 || strcmp(argv[i], "-nsnp") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.nsnp = atoi(str.c_str());
		}
		else if (strcmp(argv[i], "--block") == 0 || strcmp(argv[i], "-b") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.b = str;
		}
		else if (strcmp(argv[i], "--Heritability") == 0 || strcmp(argv[i], "-h") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.h = atof(str.c_str());
		}
		else if (strcmp(argv[i], "--Thread") == 0 || strcmp(argv[i], "-t") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.t = atoi(str.c_str());
		}
		else if (strcmp(argv[i], "--EFF") == 0 || strcmp(argv[i], "-eff") == 0) {

			if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
			++i;
			str.clear();
			str.assign(argv[i]);
			cPar.eff = str;
		}
		else if (strcmp(argv[i], "--training") == 0 || strcmp(argv[i], "-training") == 0) {

		  if (argv[i + 1] == NULL || argv[i + 1][0] == '-') { continue; }
		  ++i;
		  str.clear();
		  str.assign(argv[i]);
		  cPar.training = to_bool(str);
		}

	}
	return;
}


//  Copyright John Maddock 2007.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include <ostream>
#include <istream>
#include <cmath>
#include <NTL/RR.h>

#ifndef BOOST_MATH_NTL_RR_HPP
#define BOOST_MATH_NTL_RR_HPP

namespace boost{ namespace math{

namespace ntl
{

class RR;

RR ldexp(RR r, int exp);
RR frexp(RR r, int* exp);

class RR
{
public:
   // Constructors:
   RR() {}
   RR(const ::NTL::RR& c) : m_value(c){}
   RR(char c)
   {
      m_value = c;
   }
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   RR(wchar_t c)
   {
      m_value = c;
   }
#endif
   RR(unsigned char c)
   {
      m_value = c;
   }
   RR(signed char c)
   {
      m_value = c;
   }
   RR(unsigned short c)
   {
      m_value = c;
   }
   RR(short c)
   {
      m_value = c;
   }
   RR(unsigned int c)
   {
      assign_large_int(c);
   }
   RR(int c)
   {
      assign_large_int(c);
   }
   RR(unsigned long c)
   {
      assign_large_int(c);
   }
   RR(long c)
   {
      assign_large_int(c);
   }
#ifdef BOOST_HAS_LONG_LONG
   RR(unsigned long long c)
   {
      assign_large_int(c);
   }
   RR(long long c)
   {
      assign_large_int(c);
   }
#endif
   RR(float c)
   {
      m_value = c;
   }
   RR(double c)
   {
      m_value = c;
   }
   RR(long double c)
   {
      assign_large_real(c);
   }

   // Assignment:
   RR& operator=(char c) { m_value = c; return *this; }
   RR& operator=(unsigned char c) { m_value = c; return *this; }
   RR& operator=(signed char c) { m_value = c; return *this; }
#ifndef BOOST_NO_INTRINSIC_WCHAR_T
   RR& operator=(wchar_t c) { m_value = c; return *this; }
#endif
   RR& operator=(short c) { m_value = c; return *this; }
   RR& operator=(unsigned short c) { m_value = c; return *this; }
   RR& operator=(int c) { assign_large_int(c); return *this; }
   RR& operator=(unsigned int c) { assign_large_int(c); return *this; }
   RR& operator=(long c) { assign_large_int(c); return *this; }
   RR& operator=(unsigned long c) { assign_large_int(c); return *this; }
#ifdef BOOST_HAS_LONG_LONG
   RR& operator=(long long c) { assign_large_int(c); return *this; }
   RR& operator=(unsigned long long c) { assign_large_int(c); return *this; }
#endif
   RR& operator=(float c) { m_value = c; return *this; }
   RR& operator=(double c) { m_value = c; return *this; }
   RR& operator=(long double c) { assign_large_real(c); return *this; }

   // Access:
   NTL::RR& value(){ return m_value; }
   NTL::RR const& value()const{ return m_value; }

   // Member arithmetic:
   RR& operator+=(const RR& other)
   { m_value += other.value(); return *this; }
   RR& operator-=(const RR& other)
   { m_value -= other.value(); return *this; }
   RR& operator*=(const RR& other)
   { m_value *= other.value(); return *this; }
   RR& operator/=(const RR& other)
   { m_value /= other.value(); return *this; }
   RR operator-()const
   { return -m_value; }
   RR const& operator+()const
   { return *this; }

   // RR compatibity:
   const ::NTL::ZZ& mantissa() const
   { return m_value.mantissa(); }
   long exponent() const
   { return m_value.exponent(); }

   static void SetPrecision(long p)
   { ::NTL::RR::SetPrecision(p); }

   static long precision()
   { return ::NTL::RR::precision(); }

   static void SetOutputPrecision(long p)
   { ::NTL::RR::SetOutputPrecision(p); }
   static long OutputPrecision()
   { return ::NTL::RR::OutputPrecision(); }

private:
   ::NTL::RR m_value;

   template <class V>
   void assign_large_real(const V& a)
   {
      using std::frexp;
      using std::ldexp;
      using std::floor;
      if (a == 0) {
         clear(m_value);
         return;
      }

      if (a == 1) {
         NTL::set(m_value);
         return;
      }

      if (!(boost::math::isfinite)(a))
      {
         throw std::overflow_error("Cannot construct an instance of NTL::RR with an infinite value.");
      }

      int e;
      long double f, term;
      ::NTL::RR t;
      clear(m_value);

      f = frexp(a, &e);

      while(f)
      {
         // extract 30 bits from f:
         f = ldexp(f, 30);
         term = floor(f);
         e -= 30;
         conv(t.x, (int)term);
         t.e = e;
         m_value += t;
         f -= term;
      }
   }

   template <class V>
   void assign_large_int(V a)
   {
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4146)
#endif
      clear(m_value);
      int exp = 0;
      NTL::RR t;
      bool neg = a < V(0) ? true : false;
      if(neg)
         a = -a;
      while(a)
      {
         t = static_cast<double>(a & 0xffff);
         m_value += ldexp(RR(t), exp).value();
         a >>= 16;
         exp += 16;
      }
      if(neg)
         m_value = -m_value;
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
   }
};

// Non-member arithmetic:
inline RR operator+(const RR& a, const RR& b)
{
   RR result(a);
   result += b;
   return result;
}
inline RR operator-(const RR& a, const RR& b)
{
   RR result(a);
   result -= b;
   return result;
}
inline RR operator*(const RR& a, const RR& b)
{
   RR result(a);
   result *= b;
   return result;
}
inline RR operator/(const RR& a, const RR& b)
{
   RR result(a);
   result /= b;
   return result;
}

// Comparison:
inline bool operator == (const RR& a, const RR& b)
{ return a.value() == b.value() ? true : false; }
inline bool operator != (const RR& a, const RR& b)
{ return a.value() != b.value() ? true : false;}
inline bool operator < (const RR& a, const RR& b)
{ return a.value() < b.value() ? true : false; }
inline bool operator <= (const RR& a, const RR& b)
{ return a.value() <= b.value() ? true : false; }
inline bool operator > (const RR& a, const RR& b)
{ return a.value() > b.value() ? true : false; }
inline bool operator >= (const RR& a, const RR& b)
{ return a.value() >= b.value() ? true : false; }

#if 0
// Non-member mixed compare:
template <class T>
inline bool operator == (const T& a, const RR& b)
{
   return a == b.value();
}
template <class T>
inline bool operator != (const T& a, const RR& b)
{
   return a != b.value();
}
template <class T>
inline bool operator < (const T& a, const RR& b)
{
   return a < b.value();
}
template <class T>
inline bool operator > (const T& a, const RR& b)
{
   return a > b.value();
}
template <class T>
inline bool operator <= (const T& a, const RR& b)
{
   return a <= b.value();
}
template <class T>
inline bool operator >= (const T& a, const RR& b)
{
   return a >= b.value();
}
#endif  // Non-member mixed compare:

// Non-member functions:
/*
inline RR acos(RR a)
{ return ::NTL::acos(a.value()); }
*/
inline RR cos(RR a)
{ return ::NTL::cos(a.value()); }
/*
inline RR asin(RR a)
{ return ::NTL::asin(a.value()); }
inline RR atan(RR a)
{ return ::NTL::atan(a.value()); }
inline RR atan2(RR a, RR b)
{ return ::NTL::atan2(a.value(), b.value()); }
*/
inline RR ceil(RR a)
{ return ::NTL::ceil(a.value()); }
/*
inline RR fmod(RR a, RR b)
{ return ::NTL::fmod(a.value(), b.value()); }
inline RR cosh(RR a)
{ return ::NTL::cosh(a.value()); }
*/
inline RR exp(RR a)
{ return ::NTL::exp(a.value()); }
inline RR fabs(RR a)
{ return ::NTL::fabs(a.value()); }
inline RR abs(RR a)
{ return ::NTL::abs(a.value()); }
inline RR floor(RR a)
{ return ::NTL::floor(a.value()); }
/*
inline RR modf(RR a, RR* ipart)
{
   ::NTL::RR ip;
   RR result = modf(a.value(), &ip);
   *ipart = ip;
   return result;
}
inline RR frexp(RR a, int* expon)
{ return ::NTL::frexp(a.value(), expon); }
inline RR ldexp(RR a, int expon)
{ return ::NTL::ldexp(a.value(), expon); }
*/
inline RR log(RR a)
{ return ::NTL::log(a.value()); }
inline RR log10(RR a)
{ return ::NTL::log10(a.value()); }
/*
inline RR tan(RR a)
{ return ::NTL::tan(a.value()); }
*/
inline RR pow(RR a, RR b)
{ return ::NTL::pow(a.value(), b.value()); }
inline RR pow(RR a, int b)
{ return ::NTL::power(a.value(), b); }
inline RR sin(RR a)
{ return ::NTL::sin(a.value()); }
/*
inline RR sinh(RR a)
{ return ::NTL::sinh(a.value()); }
*/
inline RR sqrt(RR a)
{ return ::NTL::sqrt(a.value()); }
/*
inline RR tanh(RR a)
{ return ::NTL::tanh(a.value()); }
*/
   inline RR pow(const RR& r, long l)
   {
      return ::NTL::power(r.value(), l);
   }
   inline RR tan(const RR& a)
   {
      return sin(a)/cos(a);
   }
   inline RR frexp(RR r, int* exp)
   {
      *exp = r.value().e;
      r.value().e = 0;
      while(r >= 1)
      {
         *exp += 1;
         r.value().e -= 1;
      }
      while(r < 0.5)
      {
         *exp -= 1;
         r.value().e += 1;
      }
      BOOST_ASSERT(r < 1);
      BOOST_ASSERT(r >= 0.5);
      return r;
   }
   inline RR ldexp(RR r, int exp)
   {
      r.value().e += exp;
      return r;
   }

// Streaming:
template <class charT, class traits>
inline std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const RR& a)
{
   return os << a.value();
}
template <class charT, class traits>
inline std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, RR& a)
{
   ::NTL::RR v;
   is >> v;
   a = v;
   return is;
}

} // namespace ntl

namespace tools
{

template<>
inline int digits<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
   return ::NTL::RR::precision();
}

template <>
inline float real_cast<float, boost::math::ntl::RR>(boost::math::ntl::RR t)
{
   double r;
   conv(r, t.value());
   return static_cast<float>(r);
}
template <>
inline double real_cast<double, boost::math::ntl::RR>(boost::math::ntl::RR t)
{
   double r;
   conv(r, t.value());
   return r;
}

namespace detail{

template<class I>
void convert_to_long_result(NTL::RR const& r, I& result)
{
   result = 0;
   I last_result(0);
   NTL::RR t(r);
   double term;
   do
   {
      conv(term, t);
      last_result = result;
      result += static_cast<I>(term);
      t -= term;
   }while(result != last_result);
}

}

template <>
inline long double real_cast<long double, boost::math::ntl::RR>(boost::math::ntl::RR t)
{
   long double result(0);
   detail::convert_to_long_result(t.value(), result);
   return result;
}
template <>
inline boost::math::ntl::RR real_cast<boost::math::ntl::RR, boost::math::ntl::RR>(boost::math::ntl::RR t)
{
   return t;
}
template <>
inline unsigned real_cast<unsigned, boost::math::ntl::RR>(boost::math::ntl::RR t)
{
   unsigned result;
   detail::convert_to_long_result(t.value(), result);
   return result;
}
template <>
inline int real_cast<int, boost::math::ntl::RR>(boost::math::ntl::RR t)
{
   unsigned result;
   detail::convert_to_long_result(t.value(), result);
   return result;
}

template <>
inline boost::math::ntl::RR max_value<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
   static bool has_init = false;
   static NTL::RR val;
   if(!has_init)
   {
      val = 1;
      val.e = NTL_OVFBND-20;
      has_init = true;
   }
   return val;
}

template <>
inline boost::math::ntl::RR min_value<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
   static bool has_init = false;
   static NTL::RR val;
   if(!has_init)
   {
      val = 1;
      val.e = -NTL_OVFBND+20;
      has_init = true;
   }
   return val;
}

template <>
inline boost::math::ntl::RR log_max_value<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
   static bool has_init = false;
   static NTL::RR val;
   if(!has_init)
   {
      val = 1;
      val.e = NTL_OVFBND-20;
      val = log(val);
      has_init = true;
   }
   return val;
}

template <>
inline boost::math::ntl::RR log_min_value<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
   static bool has_init = false;
   static NTL::RR val;
   if(!has_init)
   {
      val = 1;
      val.e = -NTL_OVFBND+20;
      val = log(val);
      has_init = true;
   }
   return val;
}

template <>
inline boost::math::ntl::RR epsilon<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
   return ldexp(boost::math::ntl::RR(1), 1-boost::math::policies::digits<boost::math::ntl::RR, boost::math::policies::policy<> >());
}

} // namespace tools

//
// The number of digits precision in RR can vary with each call
// so we need to recalculate these with each call:
//
namespace constants{

template<> inline boost::math::ntl::RR pi<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
    NTL::RR result;
    ComputePi(result);
    return result;
}
template<> inline boost::math::ntl::RR e<boost::math::ntl::RR>(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(boost::math::ntl::RR))
{
    NTL::RR result;
    result = 1;
    return exp(result);
}

} // namespace constants

namespace ntl{
   //
   // These are some fairly brain-dead versions of the math
   // functions that NTL fails to provide.
   //

   //
   // Inverse trig functions:
   //
   struct asin_root
   {
      asin_root(RR const& target) : t(target){}

      std::tr1::tuple<RR, RR, RR> operator()(RR const& p)
      {
         RR f0 = sin(p);
         RR f1 = cos(p);
         RR f2 = -f0;
         f0 -= t;
         return std::tr1::make_tuple(f0, f1, f2);
      }
   private:
      RR t;
   };

   inline RR asin(RR z)
   {
      double r;
      conv(r, z.value());
      return boost::math::tools::halley_iterate(
         asin_root(z),
         RR(std::asin(r)),
         RR(-boost::math::constants::pi<RR>()/2),
         RR(boost::math::constants::pi<RR>()/2),
         NTL::RR::precision());
   }

   struct acos_root
   {
      acos_root(RR const& target) : t(target){}

      std::tr1::tuple<RR, RR, RR> operator()(RR const& p)
      {
         RR f0 = cos(p);
         RR f1 = -sin(p);
         RR f2 = -f0;
         f0 -= t;
         return std::tr1::make_tuple(f0, f1, f2);
      }
   private:
      RR t;
   };

   inline RR acos(RR z)
   {
      double r;
      conv(r, z.value());
      return boost::math::tools::halley_iterate(
         acos_root(z),
         RR(std::acos(r)),
         RR(-boost::math::constants::pi<RR>()/2),
         RR(boost::math::constants::pi<RR>()/2),
         NTL::RR::precision());
   }

   struct atan_root
   {
      atan_root(RR const& target) : t(target){}

      std::tr1::tuple<RR, RR, RR> operator()(RR const& p)
      {
         RR c = cos(p);
         RR ta = tan(p);
         RR f0 = ta - t;
         RR f1 = 1 / (c * c);
         RR f2 = 2 * ta / (c * c);
         return std::tr1::make_tuple(f0, f1, f2);
      }
   private:
      RR t;
   };

   inline RR atan(RR z)
   {
      double r;
      conv(r, z.value());
      return boost::math::tools::halley_iterate(
         atan_root(z),
         RR(std::atan(r)),
         -boost::math::constants::pi<RR>()/2,
         boost::math::constants::pi<RR>()/2,
         NTL::RR::precision());
   }

   inline RR sinh(RR z)
   {
      return (expm1(z.value()) - expm1(-z.value())) / 2;
   }

   inline RR cosh(RR z)
   {
      return (exp(z) + exp(-z)) / 2;
   }

   inline RR tanh(RR z)
   {
      return sinh(z) / cosh(z);
   }

   inline RR fmod(RR x, RR y)
   {
      // This is a really crummy version of fmod, we rely on lots
      // of digits to get us out of trouble...
      RR factor = floor(x/y);
      return x - factor * y;
   }

} // namespace ntl

} // namespace math
} // namespace boost

#endif // BOOST_MATH_REAL_CONCEPT_HPP


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "strand_bias.hpp"

#include <algorithm>
#include <iterator>
#include <random>
#include <functional>
#include <cmath>
#include <cassert>

#include <boost/variant.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>

#include "io/variant/vcf_record.hpp"
#include "io/variant/vcf_spec.hpp"
#include "basics/aligned_read.hpp"
#include "utils/maths.hpp"
#include "utils/beta_distribution.hpp"
#include "utils/string_utils.hpp"
#include "../facets/samples.hpp"
#include "../facets/alleles.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string StrandBias::name_ = "SB";

StrandBias::StrandBias(const double critical_value)
: min_medium_trigger_ {critical_value / 2}
, min_big_trigger_ {critical_value / 8}
, critical_resample_lb_ {0.995 * critical_value}
, critical_resample_ub_ {1.005 * critical_value}
, use_resampling_ {true}
{}

std::unique_ptr<Measure> StrandBias::do_clone() const
{
    return std::make_unique<StrandBias>(*this);
}

Measure::ValueType StrandBias::get_value_type() const
{
    return double {};
}

void StrandBias::do_set_parameters(std::vector<std::string> params)
{
    if (params.size() != 1) {
        throw BadMeasureParameters {this->name(), "only has one parameter (min proportion difference)"};
    }
    try {
        min_difference_ = boost::lexical_cast<decltype(min_difference_)>(params.front());
    } catch (const boost::bad_lexical_cast&) {
        throw BadMeasureParameters {this->name(), "given parameter \"" + params.front() + "\" cannot be parsed"};
    }
    if (min_difference_ < 0 || min_difference_ > 1) {
        throw BadMeasureParameters {this->name(), "min proportion difference must be between 0 and 1"};
    }
}

std::vector<std::string> StrandBias::do_parameters() const
{
    return {utils::to_string(min_difference_, 2)};
}

namespace {

bool is_canonical(const VcfRecord::NucleotideSequence& allele) noexcept
{
    return !(allele == vcfspec::missingValue || allele == vcfspec::deleteMaskAllele);
}

bool has_called_alt_allele(const VcfRecord& call, const VcfRecord::SampleName& sample)
{
    if (!call.has_genotypes()) return true;
    const auto& genotype = get_genotype(call, sample);
    return std::any_of(std::cbegin(genotype), std::cend(genotype),
                       [&] (const auto& allele) { return allele != call.ref() && is_canonical(allele); });
}

bool is_evaluable(const VcfRecord& call, const VcfRecord::SampleName& sample)
{
    return has_called_alt_allele(call, sample) && call.is_heterozygous(sample);
}

struct DirectionCounts
{
    unsigned forward, reverse;
};

template <typename Container>
DirectionCounts count_directions(const Container& reads, const GenomicRegion& call_region)
{
    unsigned n_forward {0}, n_reverse {0};
    for (const auto& read : reads) {
        if (overlaps(read.get(), call_region)) {
            if (is_forward_strand(read)) {
                ++n_forward;
            } else {
                ++n_reverse;
            }
        }
    }
    return {n_forward, n_reverse};
}

using DirectionCountVector = std::vector<DirectionCounts>;

auto get_direction_counts(const std::vector<Allele>& alleles, const AlleleSupportMap& support, const GenomicRegion& call_region, const unsigned prior = 1)
{
    DirectionCountVector result {};
    result.reserve(alleles.size());
    for (const auto& allele : alleles) {
        result.push_back(count_directions(support.at(allele), call_region));
        result.back().forward += prior;
        result.back().reverse += prior;
    }
    return result;
}

template <typename URNG>
auto sample_beta(const DirectionCounts& counts, const std::size_t n, URNG& generator)
{
    std::beta_distribution<> beta {static_cast<double>(counts.forward), static_cast<double>(counts.reverse)};
    std::vector<double> result(n);
    std::generate_n(std::begin(result), n, [&] () { return beta(generator); });
    return result;
}

template <typename URNG>
typename URNG::result_type
generate_urng_seed(const DirectionCountVector& direction_counts, const std::size_t num_samples) noexcept
{
    const static auto add_counts = [] (auto curr, const auto& counts) noexcept { return counts.forward + counts.reverse; };
    const auto tot_count = std::accumulate(std::cbegin(direction_counts), std::cend(direction_counts), 0u, add_counts);
    return tot_count + num_samples % tot_count;
}

auto generate_beta_samples(const DirectionCountVector& direction_counts, const std::size_t num_samples)
{
    std::mt19937 generator {generate_urng_seed<std::mt19937>(direction_counts, num_samples)};
    std::vector<std::vector<double>> result {};
    result.reserve(direction_counts.size());
    for (const auto& counts : direction_counts) {
        result.push_back(sample_beta(counts, num_samples, generator));
    }
    return result;
}

double estimate_prob_different(const std::vector<double>& lhs, const std::vector<double>& rhs,
                               const double min_diff)
{
    assert(lhs.size() == rhs.size());
    std::vector<double> diffs(lhs.size());
    std::transform(std::cbegin(lhs), std::cend(lhs), std::cbegin(rhs), std::begin(diffs), std::minus<> {});
    auto n_diffs = std::count_if(std::cbegin(diffs), std::cend(diffs), [=] (auto diff) { return std::abs(diff) > min_diff; });
    return static_cast<double>(n_diffs) / diffs.size();
}

double calculate_max_prob_different(const DirectionCountVector& direction_counts, const std::size_t num_samples,
                                    const double min_diff)
{
    const auto num_counts = direction_counts.size();
    if (num_counts < 2) return 0;
    const auto samples = generate_beta_samples(direction_counts, num_samples);
    double result {0};
    for (std::size_t i {0}; i < num_counts - 1; ++i) {
        for (auto j = i + 1; j < num_counts; ++j) {
            result = std::max(result, estimate_prob_different(samples[i], samples[j], min_diff));
        }
    }
    return result;
}

} // namespace

Measure::ResultType StrandBias::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& alleles = get_value<Alleles>(facets.at("Alleles"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).alleles;
    Array<Optional<ValueType>> result(samples.size());
    for (std::size_t s {0}; s < samples.size(); ++s) {
        const auto& sample = samples[s];
        if (is_evaluable(call, sample)) {
            const auto direction_counts = get_direction_counts(get_called(alleles, call, sample), assignments.at(sample), mapped_region(call));
            double prob;
            if (use_resampling_) {
                prob = calculate_max_prob_different(direction_counts, small_sample_size_, min_difference_);
                if (prob >= min_big_trigger_) {
                    prob = calculate_max_prob_different(direction_counts, big_sample_size_, min_difference_);
                } else if (prob >= min_medium_trigger_) {
                    prob = calculate_max_prob_different(direction_counts, medium_sample_size_, min_difference_);
                    if (prob >= min_big_trigger_) {
                        prob = calculate_max_prob_different(direction_counts, big_sample_size_, min_difference_);
                    }
                }
                if (prob > critical_resample_lb_ && prob < critical_resample_ub_) {
                    prob = calculate_max_prob_different(direction_counts, very_big_sample_size, min_difference_);
                }
            } else {
                prob = calculate_max_prob_different(direction_counts, big_sample_size_, min_difference_);
            }
            result[s] = ValueType {prob};
        }
    }
    return result;
}

Measure::ResultCardinality StrandBias::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& StrandBias::do_name() const
{
    return name_;
}

std::string StrandBias::do_describe() const
{
    return "Strand bias of reads based on haplotype support";
}

std::vector<std::string> StrandBias::do_requirements() const
{
    return {"Samples", "Alleles", "ReadAssignments"};
}

bool StrandBias::is_equal(const Measure& other) const noexcept
{
    return min_medium_trigger_ == static_cast<const StrandBias&>(other).min_medium_trigger_;
}

} // namespace csr
} // namespace octopus


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:51:19

#ifndef MSSM_PHYSICAL_H
#define MSSM_PHYSICAL_H

#include "linalg2.hpp"
#include <Eigen/Core>

#include <iosfwd>
#include <string>

namespace flexiblesusy {

struct MSSM_physical {
   MSSM_physical();
   void clear();
   void convert_to_hk();   ///< converts pole masses to HK convention
   void convert_to_slha(); ///< converts pole masses to SLHA convention
   Eigen::ArrayXd get() const; ///< returns array with all masses and mixings
   void set(const Eigen::ArrayXd&); ///< set all masses and mixings
   Eigen::ArrayXd get_masses() const; ///< returns array with all masses
   void set_masses(const Eigen::ArrayXd&); ///< set all masses
   void print(std::ostream&) const;

   double MVG;
   double MGlu;
   Eigen::Array<double,3,1> MFv;
   Eigen::Array<double,6,1> MSd;
   Eigen::Array<double,3,1> MSv;
   Eigen::Array<double,6,1> MSu;
   Eigen::Array<double,6,1> MSe;
   Eigen::Array<double,2,1> Mhh;
   Eigen::Array<double,2,1> MAh;
   Eigen::Array<double,2,1> MHpm;
   Eigen::Array<double,4,1> MChi;
   Eigen::Array<double,2,1> MCha;
   Eigen::Array<double,3,1> MFe;
   Eigen::Array<double,3,1> MFd;
   Eigen::Array<double,3,1> MFu;
   double MVWm;
   double MVP;
   double MVZ;

   Eigen::Matrix<double,6,6> ZD;
   Eigen::Matrix<double,3,3> ZV;
   Eigen::Matrix<double,6,6> ZU;
   Eigen::Matrix<double,6,6> ZE;
   Eigen::Matrix<double,2,2> ZH;
   Eigen::Matrix<double,2,2> ZA;
   Eigen::Matrix<double,2,2> ZP;
   Eigen::Matrix<std::complex<double>,4,4> ZN;
   Eigen::Matrix<std::complex<double>,2,2> UM;
   Eigen::Matrix<std::complex<double>,2,2> UP;
   Eigen::Matrix<std::complex<double>,3,3> ZEL;
   Eigen::Matrix<std::complex<double>,3,3> ZER;
   Eigen::Matrix<std::complex<double>,3,3> ZDL;
   Eigen::Matrix<std::complex<double>,3,3> ZDR;
   Eigen::Matrix<std::complex<double>,3,3> ZUL;
   Eigen::Matrix<std::complex<double>,3,3> ZUR;
   Eigen::Matrix<double,2,2> ZZ;

};

std::ostream& operator<<(std::ostream&, const MSSM_physical&);

} // namespace flexiblesusy

#endif


#include <iostream>
#include <fstream>
#include <iterator>
#include <limits.h>
#include <cmath>
#include <immintrin.h>
#include <omp.h>
#include "bayes.hpp"
#include "utilities.hpp"
#include "dotp_lut.hpp"
#include "na_lut.hpp"
#include "xfiles.hpp"
#include <boost/math/special_functions/gamma.hpp>

void Bayes::predict() {

    MPI_Barrier(MPI_COMM_WORLD);
    double ts = MPI_Wtime();

    check_openmp();

    MPI_Status status;
    MPI_Offset file_size = 0;
    MPI_File*  fh;

    cross_bim_files();

    int pidx = 0;
    for (auto& phen : pmgr.get_phens()) {
        pidx += 1;

        phen.delete_output_prediction_files();
        phen.open_prediction_files();

        const std::vector<unsigned char> mask4 = phen.get_mask4();
        const int im4 = phen.get_im4();

        fh = phen.get_inbet_fh();
        check_mpi(MPI_File_get_size(*fh, &file_size), __LINE__, __FILE__);
        //printf("file_size = %u B\n", file_size);

        // First element of the .bet is the total number of processed markers
        // Then: iteration (uint) beta (double) for all markers
        uint Mtot_ = 0;
        MPI_Offset betoff = size_t(0);
        check_mpi(MPI_File_read_at_all(*fh, betoff, &Mtot_, 1, MPI_UNSIGNED, &status), __LINE__, __FILE__);
        if (Mtot_ != m_refrsid.size()) {
            printf("Mismatch between expected and Mtot read from .bet file: %lu vs %d\n", rsid.size(), Mtot_);
            MPI_Abort(MPI_COMM_WORLD, 1);
        }

        assert((file_size - sizeof(uint)) % (Mtot_ * sizeof(double) + sizeof(uint)) == 0);
        uint niter = (file_size - sizeof(uint)) / (Mtot_ * sizeof(double) + sizeof(uint));
        if (rank == 0)
            printf("INFO   : Number of recorded iterations in .bet file %d: %u\n", pidx-1, niter);

        double* beta_sum = (double*) _mm_malloc(size_t(Mtot_) * sizeof(double), 32);
        check_malloc(beta_sum, __LINE__, __FILE__);
        for (int i=0; i<Mtot_; i++) beta_sum[i] = 0.0;

        double* beta_it = (double*) _mm_malloc(size_t(Mtot_) * sizeof(double), 32);
        check_malloc(beta_it, __LINE__, __FILE__);

        uint start_iter = 0;
        //EO: use this one to speed up testing (avoids to read the entire bet history)
        //if (niter > 3) start_iter = niter - 3;

        for (uint i=start_iter; i<niter; i++) {
            betoff
                = sizeof(uint) // Mtot
                + (sizeof(uint) + size_t(Mtot_) * sizeof(double)) * size_t(i)
                + sizeof(uint);
            check_mpi(MPI_File_read_at_all(*fh, betoff, beta_it, Mtot_, MPI_DOUBLE, &status), __LINE__, __FILE__);
            for (int j=0; j<Mtot_;j++)
                beta_sum[j] += beta_it[j];
        }

        for (int j=0; j<Mtot_;j++)
            beta_sum[j] /= double(niter);

        fflush(stdout);
        double t_1 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 1 = %.2f seconds.\n", t_1 - ts);

        double* g_k = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
        check_malloc(g_k, __LINE__, __FILE__);
        for (int i=0; i<im4*4; i++) g_k[i] = 0.0;

#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int mrki=0; mrki<M; mrki++) {

            // Get rsid from current
            int mglo = S + mrki;
            std::string id = rsid.at(mglo);

            // Skip markers with no corresponding rsid in reference bim file
            if (m_refrsid.find(id) == m_refrsid.end()) {
                //printf("%d -> %d = %s not found in reference bim\n", mrki, mglo, id.c_str());
                continue;
            }

            int rmglo = m_refrsid.find(id)->second; // global marker index in reference bed

            size_t bedix = size_t(mrki) * size_t(mbytes);
            const unsigned char* bedm = &bed_data[bedix];

            double mave = phen.get_marker_ave(mrki);
            double msig = phen.get_marker_sig(mrki);

            for (int j=0; j<im4; j++) {
                for (int k=0; k<4; k++) {
                    double val = (dotp_lut_a[bedm[j] * 4 + k] - mave) * dotp_lut_b[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k] * msig;
#ifdef _OPENMP
#pragma omp atomic update
#endif
                    g_k[j*4+k] += val * beta_sum[mglo];
                }
            }
        }

        fflush(stdout);
        double t_2 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 2 = %.2f seconds.\n", t_2 - t_1);

        double* g = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
        check_malloc(g, __LINE__, __FILE__);

        check_mpi(MPI_Allreduce(g_k, g, im4*4, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD), __LINE__, __FILE__);

        double* y_k = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
        check_malloc(y_k, __LINE__, __FILE__);

        phen.get_centered_and_scaled_y(y_k);

        //EO: already done when computing original epsilon when reading .phen
	//phen.set_nas_to_zero(y_k, im4*4);
        // NAs are 0.0 in all, so should be safe
        for (int i=0; i<im4*4; i++)
            y_k[i] -= (g[i] - g_k[i]);

        double sigma = 0.0;
        for (int i=0; i<N; i++)
            sigma += y_k[i] * y_k[i];
        sigma /= phen.get_nonas();
        //printf("### r: %d sigma = %20.15f\n", rank, sigma);

        MPI_File* mlma_fh = phen.get_outmlma_fh();

        double* Beta  = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Beta, __LINE__, __FILE__);
        double* Tdist = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Tdist, __LINE__, __FILE__);
        double* Se    = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Se, __LINE__, __FILE__);
        double* Pval  = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Pval, __LINE__, __FILE__);

#ifdef _OPENMP
#pragma omp parallel for
#endif
        for (int mrki=0; mrki<M; mrki++) {

            // Global index in current .bim
            int mglo = S + mrki;
            std::string id = rsid.at(mglo);

            // Skip markers with no corresponding rsid in reference bim file
            if (m_refrsid.find(id) == m_refrsid.end()) {
                //printf("%d -> %d = %s not found in reference bim\n", mrki, mglo, id.c_str());
                continue;
            }

            // Global marker index in reference bed
            int rmglo = m_refrsid.find(id)->second;

            size_t bedix = size_t(mrki) * size_t(mbytes);
            const unsigned char* bedm = &bed_data[bedix];

            double mave = phen.get_marker_ave(mrki);
            double msig = phen.get_marker_sig(mrki);

            double xtx = 0.0;
            double xty = 0.0;
            double chk = 0.0;
            for (int j=0; j<im4; j++) {
                for (int k=0; k<4; k++) {
                    double val = dotp_lut_a[bedm[j] * 4 + k] * dotp_lut_b[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k];
                    xtx += val * val;
                    xty += val * y_k[j*4 + k];
                }
            }

            double beta  = xty / xtx;
            double tdist = xty / sqrt(sigma * xtx);
            double se    = beta / tdist;
            double pval  = 1.0 - boost::math::gamma_p(0.5, tdist * tdist * 0.5);

            Beta[mrki]  = beta;
            Tdist[mrki] = tdist;
            Se[mrki]    = se;
            Pval[mrki]  = pval;

            //if (mrki < 5) {
            //    printf("r: %3d  m: %8d %8d (%5s) %8d  beta=%20.15f  se=%20.15f  tdist=%20.15f pval=%20.15f   xtx=%10.1f xty=%15.6f\n", rank, mrki, mglo, id.c_str(), rmglo, beta, se, tdist, pval, xtx, xty);
            //}
        }

        fflush(stdout);
        double t_3 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 3 = %.2f seconds.\n", t_3 - t_2);

        const int LLEN = 123 + 1;
        char* todump  = (char*) _mm_malloc(size_t(LLEN) * size_t(M) * sizeof(char), 32);
        check_malloc(todump, __LINE__, __FILE__);

        int n_rem = 0;
        for (int mrki=0; mrki<M; mrki++) {
            int mglo = S + mrki;
            std::string id = rsid.at(mglo);
            if (m_refrsid.find(id) == m_refrsid.end()) {
                //|| id.compare("rs12562034") == 0m|| id.compare("rs188466450") == 0)
                printf("WARNING: marker id %s excluded -- no match\n", id.c_str());
                n_rem++;
                continue;
            }
            int rmglo = m_refrsid.find(id)->second;
            int cx = snprintf(&todump[(mrki - n_rem) * (LLEN - 1)], LLEN, "%20s %8d %8d %20.15f %20.15f %20.15f %20.15f\n",
                              id.c_str(), mglo, rmglo, Beta[mrki], Tdist[mrki], Se[mrki], Pval[mrki]);
            assert(cx >= 0 && cx < LLEN);
        }

        // Collect numbers of markers to print in each task
        int mp = M - n_rem;
        int* mps = (int*) malloc(nranks * sizeof(int));
        check_mpi(MPI_Allgather(&mp, 1, MPI_INTEGER, mps, 1, MPI_INTEGER, MPI_COMM_WORLD), __LINE__, __FILE__);

        int ps = 0;
        for (int i=0; i<rank; i++) { ps += mps[i]; }

        MPI_Offset offset = size_t(ps) * size_t(LLEN-1);
        check_mpi(MPI_File_write_at(*mlma_fh,
                                    offset, todump, size_t(LLEN-1) * size_t(mp), MPI_CHAR, &status),
                  __LINE__, __FILE__);

        _mm_free(todump);

        fflush(stdout);
        double t_4 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 4 = %.2f seconds.\n", t_4 - t_3);

        MPI_Barrier(MPI_COMM_WORLD);

        phen.close_prediction_files();

        _mm_free(Beta);
        _mm_free(Tdist);
        _mm_free(Se);
        _mm_free(Pval);
        _mm_free(beta_sum);
        _mm_free(beta_it);
        _mm_free(g_k);
        _mm_free(g);
        _mm_free(y_k);
    }

    fflush(stdout);
    double te = MPI_Wtime();
    MPI_Barrier(MPI_COMM_WORLD);
    if (rank == 0)
        printf("INFO   : Time to compute the predictions: %.2f seconds.\n", te - ts);
}

// EO: bim_file - I assume that the row number is the index
//
void Bayes::cross_bim_files() {

    if (rank == 0) {
        printf("INFO   : bim file:     %s\n", opt.get_bim_file().c_str());
        printf("INFO   : ref bim file: %s\n", opt.get_ref_bim_file().c_str());
    }
    std::ifstream in(opt.get_bim_file().c_str());
    if (!in) throw ("Error: can not open the file [" + opt.get_bim_file() + "] to read.");
    std::string   id, allele1, allele2;
    unsigned chr, physPos, idx = 0;
    float    genPos;
    while (in >> chr >> id >> genPos >> physPos >> allele1 >> allele2) {
        rsid.push_back(id);
    }
    in.close();
    int nrsid = rsid.size();
    if (rank == 0)
        printf("INFO   : found %d ids in bim file\n", nrsid);

    std::ifstream refin(opt.get_ref_bim_file().c_str());
    if (!refin) throw ("Error: can not open the file [" + opt.get_ref_bim_file() + "] to read.");
    idx = 0;
    while (refin >> chr >> id >> genPos >> physPos >> allele1 >> allele2) {
        m_refrsid[id] = idx++;
    }
    refin.close();
    if (rank == 0)
        printf("INFO   : found %d ids in reference bim file\n", idx);
}

void Bayes::process() {

    check_openmp();

    for (auto& phen : pmgr.get_phens()) {
        phen.delete_output_files();
        phen.open_output_files();
        phen.set_midx();
        for (int i=0; i<opt.get_ngroups(); i++) {
            phen.set_sigmag_for_group(i, phen.sample_beta_rng(1.0, 1.0));
            if (mtotgrp.at(i) == 0)
                phen.set_sigmag_for_group(i, 0.0);
            //printf("sample sigmag[%d] = %20.15f\n", i, phen.get_sigmag_for_group(i));
        }
        check_mpi(MPI_Bcast(phen.get_sigmag()->data(), phen.get_sigmag()->size(), MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
        phen.set_pi_est(pi_prior);
        //printf("sample sigmag[0] = %20.15f\n", phen.get_sigmag()->at(0));
    }

    const int NPHEN =  pmgr.get_phens().size();
    bool recv_update[nranks];

    for (unsigned int it = 1; it <= opt.get_iterations(); it++) {

        double ts_it = MPI_Wtime();

        if (rank == 0)
            printf("\n\n@@@ ITERATION %5d\n", it);

        int pidx = 0;
        for (auto& phen : pmgr.get_phens()) {
            pidx += 1;
            //printf("phen %d has mu = %20.15f\n", pidx, phen.get_mu());
            phen.offset_epsilon(phen.get_mu());
            //phen.update_epsilon_sum();
            if (it == 1) {
                phen.update_epsilon_sigma();
                //printf("epssum = %20.15f, sigmae = %20.15f\n", phen.get_epsilon_sum(), phen.get_sigmae());
            }
            phen.set_mu(phen.sample_norm_rng());
            //printf("new mu = %20.15f\n", phen.get_mu());
            phen.offset_epsilon(-phen.get_mu());
            //**BUG in original phen.epsilon_stats();

            // Shuffling of the markers on its own PRNG (see README/wiki)
            if (opt.shuffle_markers())
                phen.shuffle_midx(opt.mimic_hydra());

            phen.reset_m0();
            phen.reset_cass();
        }
        fflush(stdout);

        double dbetas[NPHEN * 3]; // [ dbeta:mave:msig | ... | ]

        double t_it_sync = 0.0;

        for (int mrki=0; mrki<Mm; mrki++) {

            bool share_mrk = false;
            for (int i=0; i<NPHEN*3; i++)
                dbetas[i] = 0.0;
            int mloc = 0;

            if (mrki < M) {

                mloc = pmgr.get_phens()[0].get_marker_local_index(mrki);
                const int mglo = S + mloc;
                const int mgrp = get_marker_group(mglo);
                //std::cout << "mloc = " << mloc << ", mglo = " << mglo << ", mgrp = " << mgrp << std::endl;

                int pheni = -1;

                for (auto& phen : pmgr.get_phens()) {

                    pheni += 1;

                    // Adav
                    if (phen.get_sigmag_for_group(mgrp) == 0.0) {
                        phen.set_marker_acum(mloc, 1.0);
                        phen.set_marker_beta(mloc, 0.0);
                        continue;
                    }

                    double beta   = phen.get_marker_beta(mloc);
                    double sige_g = phen.get_sigmae() / phen.get_sigmag_for_group(get_marker_group(mglo));
                    double sigg_e = 1.0 / sige_g;
                    double inv2sige = 1.0 / (2.0 * phen.get_sigmae());
                    //if (mrki < 3)
                    //    printf("mrk = %3d has beta = %20.15f; sige_g = %20.15f = %20.15f / %20.15f\n", mloc, phen.get_marker_beta(mloc), sige_g, phen.get_sigmae(), phen.get_sigmag());

                    std::vector<double> denom = phen.get_denom();
                    std::vector<double> muk   = phen.get_muk();
                    std::vector<double> logl  = phen.get_logl();

                    for (int i=1; i<=K-1; ++i) {
                        denom.at(i-1) = (double)(N - 1) + sige_g * cvai[mgrp][i];
                        //printf("it %d, rank %d, m %d: denom[%d] = %20.15f, cvai = %20.15f\n", it, rank, mloc, i-1, denom.at(i-1), cvai[mgrp][i]);
                    }

                    double num = dot_product(mloc, phen.get_epsilon(), phen.get_marker_ave(mloc), phen.get_marker_sig(mloc));

                    //printf("num = %20.15f\n", num);
                    num += beta * double(phen.get_nonas() - 1);

                    //printf("i:%d r:%d m:%d: num = %.17g, %20.15f, %20.15f\n", it, rank, mloc, num, phen.get_marker_ave(mloc), phen.get_marker_sig(mloc));

                    for (int i=1; i<=K-1; ++i)
                        muk.at(i) = num / denom.at(i - 1);

                    for (int i=0; i<K; i++) {
                        logl[i] = log(phen.get_pi_est(mgrp, i));
                        if (i>0)
                            logl[i] += -0.5 * log(sigg_e * double(phen.get_nonas() - 1) * cva[mgrp][i] + 1.0) + muk[i] * num * inv2sige;
                        //printf("logl[%d] = %20.15f\n", i, logl[i]);
                    }

                    double prob = phen.sample_unif_rng();

                    bool zero_acum = false;
                    double tmp1 = 0.0;
                    for (int i=0; i<K; i++) {
                        if (abs(logl[i] - logl[0]) > 700.0)
                            zero_acum = true;
                        tmp1 += exp(logl[i] - logl[0]);
                    }
                    zero_acum ? tmp1 = 0.0 : tmp1 = 1.0 / tmp1;
                    phen.set_marker_acum(mloc, tmp1);
                    //printf("i:%d r:%d m:%d p:%d  num = %20.15f, acum = %20.15f, prob = %20.15f\n", it, rank, mloc, pheni, num, phen.get_marker_acum(mloc), prob);

                    double dbeta = phen.get_marker_beta(mloc);

                    for (int i=0; i<K; i++) {
                        if (prob <= phen.get_marker_acum(mloc) || i == K - 1) {
                            if (i == 0) {
                                phen.set_marker_beta(mloc, 0.0);
                                //printf("@0@ i:%4d r:%4d m:%4d: beta reset to 0.0\n", it, rank, mloc);
                            } else {
                                phen.set_marker_beta(mloc, phen.sample_norm_rng(muk[i], phen.get_sigmae() / denom[i-1]));
                                //printf("@B@ i:%4d r:%4d m:%4d:  dbetat = %20.15f, muk[%4d] = %15.10f with prob=%15.10f <= acum = %15.10f, denom = %15.10f, sigmaE = %15.10f: beta = %15.10f\n", it, rank, mloc, phen.get_marker_beta(mloc) - dbeta, i, muk[i], prob, phen.get_marker_acum(mloc), denom[i-1], phen.get_sigmae(), phen.get_marker_beta(mloc));
                                //fflush(stdout);
                            }
                            phen.increment_cass(mgrp, i, 1);
                            //std::cout << "cass " << mgrp << " " << i << " = " << phen.get_cass_for_group(mgrp,i) << std::endl;
                            phen.set_comp(mloc, i);
                            break;
                        } else {
                            bool zero_inc = false;
                            for (int j=i+1; j<K; j++) {
                                if (abs(logl[j] - logl[i+1]) > 700.0)
                                    zero_inc = true;
                            }
                            if (!zero_inc) {
                                double esum = 0.0;
                                for (int k=0; k<logl.size(); k++)
                                    esum += exp(logl[k] - logl[i+1]);
                                phen.set_marker_acum(mloc, phen.get_marker_acum(mloc) + 1.0 / esum);
                            }
                        }
                    }

                    dbeta -= phen.get_marker_beta(mloc);

                    //printf("iteration %3d, rank %3d, marker %5d: dbeta = %20.15f, pheni = %d, %20.15f %20.15f\n", it, rank, mloc, dbeta, pheni, dbetas[pheni + 1], dbetas[pheni + 2]);

                    if (abs(dbeta) > 0.0) {
                        share_mrk = true;
                        dbetas[pheni * 3 + 0] = dbeta;
                        dbetas[pheni * 3 + 1] = phen.get_marker_ave(mloc);
                        dbetas[pheni * 3 + 2] = phen.get_marker_sig(mloc);
                        //printf("@¢@ i:%3d r:%3d m:%5d: dbeta = %20.15f, pheni = %d, %20.15f %20.15f\n", it, rank, mloc, dbeta, pheni, dbetas[pheni * 3 + 1], dbetas[pheni * 3 + 2]);
                    } else {
                        //printf("-¢- i:%3d r:%3d m:%5d: dbeta = %20.15f, pheni = %d, %20.15f %20.15f\n", it, rank, mloc, dbeta, pheni, dbetas[pheni * 3 + 1], dbetas[pheni * 3 + 2]);
                    }
                }
            }

            MPI_Barrier(MPI_COMM_WORLD);
            double ts_sync = MPI_Wtime();

            // Collect information on which tasks need to share its just processed marker;
            // Tasks with M < Mm, share_mrk is false by default.
            MPI_Allgather(&share_mrk,  1, MPI_C_BOOL,
                          recv_update, 1, MPI_C_BOOL,
                          MPI_COMM_WORLD);

            /*
            if (rank == 0) {
                printf("rank:share mrk %d?: ", mrki);
                for (int i=0; i<nranks; i++) {
                    printf("%d:%d ", i, recv_update[i] == true ? 1:0);
                }
                printf("\n");
            }
            fflush(stdout);
            */

            int totbytes = 0;

            int dis_bet[nranks], cnt_bet[nranks];
            int dis_bed[nranks], cnt_bed[nranks];
            int disp_bet = 0, disp_bed = 0;
            for (int i=0; i<nranks; i++) {
                if (recv_update[i]) {
                    cnt_bet[i] = NPHEN * 3;
                    cnt_bed[i] = mbytes;
                    totbytes += mbytes;
                } else {
                    cnt_bet[i] = 0;
                    cnt_bed[i] = 0;
                }
                dis_bet[i] = disp_bet;
                dis_bed[i] = disp_bed;
                //printf("mrki = %d, recv_update[%d] = %s %d:%d\n", mrki, i, recv_update[i] ? "T" : "F", dis_bet[i], cnt_bet[i]);
                disp_bet += cnt_bet[i];
                disp_bed += cnt_bed[i];
            }

            //printf("rank %d bytes vs %d\n", totbytes, disp_bed);

            double recv_dbetas[disp_bet];
            MPI_Allgatherv(&dbetas, share_mrk ? NPHEN * 3 : 0, MPI_DOUBLE,
                           recv_dbetas, cnt_bet, dis_bet, MPI_DOUBLE, MPI_COMM_WORLD);

            unsigned char* recv_bed = (unsigned char*) _mm_malloc(disp_bed, 32);
            check_malloc(recv_bed, __LINE__, __FILE__);
            MPI_Allgatherv(&bed_data[mloc * mbytes], share_mrk ? mbytes : 0, MPI_UNSIGNED_CHAR,
                           recv_bed, cnt_bed, dis_bed, MPI_UNSIGNED_CHAR, MPI_COMM_WORLD);

            update_epsilon(cnt_bet, recv_dbetas, recv_bed);

            MPI_Barrier(MPI_COMM_WORLD);
            double te_sync = MPI_Wtime();
            t_it_sync += te_sync - ts_sync;

            _mm_free(recv_bed);

        } // End marker loop

        //exit(0);
        //continue;

        //MPI_Barrier(MPI_COMM_WORLD);

        int pheni = -1;
        for (auto& phen : pmgr.get_phens()) {
            pheni++;

            phen.reset_beta_sqn_to_zero();
            for (int i=0; i<M; i++) {
                phen.increment_beta_sqn(get_marker_group(S + i), phen.get_marker_beta(i) * phen.get_marker_beta(i));
            }

            //for (int i=0; i<G; i++)
            //    printf("i:%d r:%d p:%d g:%d: beta_sqn[%d] = %20.15f\n", it, rank, pheni, i, i, phen.get_beta_sqn_for_group(i));

            double* beta_sqn_sum = (double*) malloc(G * sizeof(double));
            check_mpi(MPI_Allreduce(phen.get_beta_sqn()->data(),
                                    beta_sqn_sum,
                                    G,
                                    MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD), __LINE__, __FILE__);
            phen.set_beta_sqn(beta_sqn_sum);
            free(beta_sqn_sum);

            int* cass_sum = (int*) malloc(G * K * sizeof(int));
            check_mpi(MPI_Allreduce(phen.get_cass(),
                                    cass_sum,
                                    G * K,
                                    MPI_INT,
                                    MPI_SUM,
                                    MPI_COMM_WORLD), __LINE__, __FILE__);
            phen.set_cass(cass_sum);
            free(cass_sum);

            // Update global parameters
            //
            for (int i=0; i<G; i++) {

                // Skip empty groups
                if (mtotgrp.at(i) == 0)
                    continue;

                //printf(" !!! i:%d r:%d p:%d g:%d:  OLD sigmag = %20.15f\n", it, rank, pheni, i, phen.get_sigmag_for_group(i));

                //if (rank == 0)
                //    printf("i:%d r:%d p:%d g:%d:  m0 = %d - %d = %d\n", it, rank, pheni, i, mtotgrp.at(i), phen.get_cass_for_group(i, 0), mtotgrp.at(i) - phen.get_cass_for_group(i, 0));

                phen.set_m0_for_group(i, mtotgrp.at(i) - phen.get_cass_for_group(i, 0));

                // Skip groups with m0 being null or empty cass (adaV in action)
                if (phen.get_m0_for_group(i) == 0 || phen.get_cass_sum_for_group(i) == 0) {
                    phen.set_sigmag_for_group(i, 0.0);
                    continue;
                }

                phen.set_sigmag_for_group(i, phen.sample_inv_scaled_chisq_rng(V0G + (double) phen.get_m0_for_group(i), (phen.get_beta_sqn_for_group(i) * (double) phen.get_m0_for_group(i) + V0G * S02G) / (V0G + (double) phen.get_m0_for_group(i))));
                //printf(" !!! i:%d r:%d p:%d g:%d:  NEW sigmag = %20.15f %20.15f\n", it, rank, pheni, i, phen.get_sigmag_for_group(i), phen.get_beta_sqn_for_group(i));

                phen.update_pi_est_dirichlet(i);
            }

            //continue;

            //if (rank == 0)
            //    phen.print_cass(mtotgrp);

            // Broadcast sigmaG of rank 0
            check_mpi(MPI_Bcast(phen.get_sigmag()->data(), phen.get_sigmag()->size(), MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
            for (int i=0; i<opt.get_ngroups(); i++) {
                //printf("i:%d r:%d p:%d g:%d:  sigmag = %20.15f\n", it, rank, pheni, i, phen.get_sigmag_for_group(i));
            }

            double e_sqn = phen.epsilon_sumsqr();
            //printf("i:%d r:%d p:%d  e_sqn = %20.15f\n", it, rank, pheni, e_sqn);

            //EO: sample sigmaE and broadcast the one from rank 0 to all the others
            phen.set_sigmae(phen.sample_inv_scaled_chisq_rng(V0E + (double)N, (e_sqn + V0E * S02E) / (V0E + (double)N)));

            double sigmae_r0 = phen.get_sigmae();
            check_mpi(MPI_Bcast(&sigmae_r0, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
            phen.set_sigmae(sigmae_r0);
            if (rank % 10 == 0) {
                printf("RESULT : i:%d r:%d p:%d  sum sigmaG = %20.15f  sigmaE = %20.15f\n", it, rank, pheni, phen.get_sigmag_sum(), phen.get_sigmae());
                //for (int i=0; i<opt.get_ngroups(); i++) {
                //    printf("i:%d r:%d p:%d  sigmaG = %20.15f  sigmaE = %20.15f\n", it, rank, pheni, phen.get_sigmag_for_group(i), phen.get_sigmae());
                //}
            }

            // Broadcast pi_est from rank 0
            for (int i=0; i<G; i++) {
                check_mpi(MPI_Bcast(phen.get_pi_est()->at(i).data(), phen.get_pi_est()->at(i).size(), MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
            }
        }

        double te_it = MPI_Wtime();
        if (rank == 0)
            printf("RESULT : It %d  total proc time = %7.3f sec, with sync time = %7.3f\n", it, te_it - ts_it, t_it_sync);

        // Write output files
        if (it % opt.get_output_thin_rate() == 0) {
            const unsigned nthinned = it / opt.get_output_thin_rate() - 1;
            for (auto& phen : pmgr.get_phens()) {
                if (rank == 0) {
                    //phen.print_pi_est();
                    write_ofile_csv(*(phen.get_outcsv_fh()), it,  phen.get_sigmag(), phen.get_sigmae(), phen.get_m0_sum(), nthinned, phen.get_pi_est());
                }
                write_ofile_h1(*(phen.get_outbet_fh()), rank, Mt, it, nthinned, S, M, phen.get_betas().data(), MPI_DOUBLE);
                write_ofile_h1(*(phen.get_outcpn_fh()), rank, Mt, it, nthinned, S, M, phen.get_comp().data(),  MPI_INTEGER);
            }
        }

    } // End iteration loop

    MPI_Barrier(MPI_COMM_WORLD);

    for (auto& phen : pmgr.get_phens())
        phen.close_output_files();
}

// counts[rank]: holds either NPHEN * 3 or 0

void Bayes::update_epsilon(const int* counts, const double* dbetas, const unsigned char* bed) {

    const int NPHEN =  pmgr.get_phens().size();

    int cnt_tot = 0;
    int bedi = 0;
    for (int i=0; i<nranks; i++) {
        int cnt = counts[i];
        //printf("r:%d  cnt = %d\n", i, cnt);
        if (cnt == 0) continue;
        assert(cnt == NPHEN * 3);
        //printf(" cnt > 0: task %d, get for rank %d cnt = %d dbetas:\n", rank, i, cnt);
        int pheni = 0;
        for (auto& phen : pmgr.get_phens()) {
            //printf("r:%d p:%d - dbeta = %20.15f\n", rank, pheni, dbetas[cnt_tot + pheni * 3]);
            if (dbetas[cnt_tot + pheni * 3] != 0.0)
                phen.update_epsilon(&dbetas[cnt_tot + pheni * 3], &bed[bedi * mbytes]);
            //phen.update_epsilon_sum();
            //printf(" - r:%d p:%d   epssum = %.17g, cnt_tot = %d\n", i, pheni, phen.get_epsilon_sum(), cnt_tot);
            pheni += 1;
        }
        cnt_tot += cnt;
        bedi    += 1;
    }
    fflush(stdout);
}

// EO, review!!
double Bayes::dot_product(const int mloc, double* __restrict__ phen, const double mu, const double sigma_inv) {

    unsigned char* bed = &bed_data[mloc * mbytes];

#ifdef MANVEC
    __m256d luta, lutb; //, lutna;
    __m512d lutab, p42;
    __m256d p4   = _mm256_set1_pd(0.0);
    __m256d suma = _mm256_set1_pd(0.0);
    __m256d sumb = _mm256_set1_pd(0.0);
    __m512d sum42 = _mm512_set1_pd(0.0);

#ifdef _OPENMP
    //#pragma omp parallel for schedule(static) reduction(addpd4:suma,sumb)
#pragma omp parallel for schedule(static) reduction(addpd8:sum42)
#endif
    for (int j=0; j<mbytes; j++) {
        //luta  = _mm256_load_pd(&dotp_lut_a[bed[j] * 4]);
        //lutb  = _mm256_load_pd(&dotp_lut_b[bed[j] * 4]);
        //luta  = _mm256_load_pd(&dotp_lut_ab[bed[j] * 8]);
        //lutb  = _mm256_load_pd(&dotp_lut_ab[bed[j] * 8 + 4]);
        lutab = _mm512_load_pd(&dotp_lut_ab[bed[j] * 8]);
        //p4    = _mm256_load_pd(&phen[j * 4]);
        p42 = _mm512_broadcast_f64x4(_mm256_load_pd(&phen[j * 4]));
        ////lutna = _mm256_load_pd(&na_lut[mask4[j] * 4]); // phen = 0.0 on NAs!
        //luta  = _mm256_mul_pd(luta, p4);
        //lutb  = _mm256_mul_pd(lutb, p4);
        p42 = _mm512_mul_pd(p42, lutab);
        //suma  = _mm256_add_pd(suma, luta);
        //sumb  = _mm256_add_pd(sumb, lutb);
        sum42 = _mm512_add_pd(sum42, p42);
    }

    //return sigma_inv *
    //    (suma[0] + suma[1] + suma[2] + suma[3] - mu * (sumb[0] + sumb[1] + sumb[2] + sumb[3]));
    return sigma_inv *
        (sum42[0] + sum42[1] + sum42[2] + sum42[3] - mu * (sum42[4] + sum42[5] + sum42[6] + sum42[7]));

#else

    double dpa = 0.0;
    double dpb = 0.0;

#ifdef _OPENMP
    //#pragma omp parallel for schedule(static) reduction(addpd4:suma,sumb)
#pragma omp parallel for schedule(static) reduction(+:dpa,dpb)
#endif
    for (int i=0; i<mbytes; i++) {
#ifdef _OPENMP
#pragma omp simd aligned(dotp_lut_a,dotp_lut_b,phen:32)
#endif
        for (int j=0; j<4; j++) {
            dpa += dotp_lut_a[bed[i] * 4 + j] * phen[i * 4 + j];
            dpb += dotp_lut_b[bed[i] * 4 + j] * phen[i * 4 + j];
        }
    }

    return sigma_inv * (dpa - mu * dpb);

#endif

}

// Setup processing: load input files and define MPI task workload
void Bayes::setup_processing() {

    mbytes = (N %  4) ? (size_t) N /  4 + 1 : (size_t) N /  4;
    load_genotype();

    pmgr.read_phen_files(opt, get_N(), get_M());

    check_processing_setup();

    if (rank == 0)
        printf("INFO   : output directory: %s\n", opt.get_out_dir().c_str());

    MPI_Barrier(MPI_COMM_WORLD);
    double ts = MPI_Wtime();
    pmgr.compute_markers_statistics(bed_data, get_N(), get_M(), mbytes);
    MPI_Barrier(MPI_COMM_WORLD);
    double te = MPI_Wtime();
    if (rank == 0)
        printf("INFO   : Time to compute the markers' statistics: %.2f seconds.\n", te - ts);

    if (opt.predict()) return;

    for (auto& phen : pmgr.get_phens()) {
        phen.set_prng_m((unsigned int)(opt.get_seed() + (rank + 0)));
        if (opt.mimic_hydra()) {
            phen.set_prng_d((unsigned int)(opt.get_seed() + (rank + 0) * 1000));
        } else {
            phen.set_prng_d((unsigned int)(opt.get_seed() + (rank + 1) * 1000));
        }
    }

    read_group_index_file(opt.get_group_index_file());

    for (int i=0; i<Mt; i++) {
        mtotgrp.at(get_marker_group(i)) += 1;
    }
    //for (int i=0; i<G; i++)
    //    printf("mtotgrp at %d = %d\n", i, mtotgrp.at(i));
}

void Bayes::check_openmp() {
#ifdef _OPENMP
#pragma omp parallel
    {
        if (rank == 0) {
            int nt = omp_get_num_threads();
            if (omp_get_thread_num() == 0)
                printf("INFO   : OMP parallel regions will use %d thread(s)\n", nt);
        }
    }
#else
    printf("WARNING: no OpenMP support!\n");
#endif
}

void Bayes::read_group_index_file(const std::string& file) {

    std::ifstream infile(file.c_str());
    if (! infile)
        throw ("Error: can not open the group file [" + file + "] to read. Use the --group-index-file option!");

    if (rank == 0)
        std::cout << "INFO   : Reading groups from " + file + "." << std::endl;

    std::string label;
    int group;

    group_index.clear();

    while (infile >> label >> group) {
        //std::cout << label << " - " << group << std::endl;
        if (group > G) {
            printf("FATAL  : group index file contains a value that exceeds the number of groups given in group mixture file.\n");
            printf("       : check the consistency between your group index and mixture input files.\n");
            exit(1);
        }
        group_index.push_back(group);
    }
}

void Bayes::check_processing_setup() {
    for (const auto& phen : pmgr.get_phens()) {
        const int Np = phen.get_nas() + phen.get_nonas();
        if (Np != N) {
            std::cout << "Fatal: N = " << N << " while phen file " << phen.get_filepath() << " has " << Np << " individuals!" << std::endl;
            exit(1);
        }
    }
}

void Bayes::load_genotype() {

    double ts = MPI_Wtime();

    MPI_File bedfh;
    const std::string bedfp = opt.get_bed_file();
    check_mpi(MPI_File_open(MPI_COMM_WORLD, bedfp.c_str(), MPI_MODE_RDONLY, MPI_INFO_NULL, &bedfh),  __LINE__, __FILE__);

    const size_t size_bytes = size_t(M) * size_t(mbytes) * sizeof(unsigned char);

    bed_data = (unsigned char*)_mm_malloc(size_bytes, 64);
    check_malloc(bed_data, __LINE__, __FILE__);
    printf("INFO   : rank %4d has allocated %zu bytes (%.3f GB) for raw data.\n", rank, size_bytes, double(size_bytes) / 1.0E9);

    // Offset to section of bed file to be processed by task
    MPI_Offset offset = size_t(3) + size_t(S) * size_t(mbytes) * sizeof(unsigned char);

    // Gather the sizes to determine common number of reads
    size_t max_size_bytes = 0;
    check_mpi(MPI_Allreduce(&size_bytes, &max_size_bytes, 1, MPI_UNSIGNED_LONG_LONG, MPI_MAX, MPI_COMM_WORLD), __LINE__, __FILE__);

    const int NREADS = check_int_overflow(size_t(ceil(double(max_size_bytes)/double(INT_MAX/2))), __LINE__, __FILE__);
    size_t bytes = 0;
    mpi_file_read_at_all <unsigned char*> (size_bytes, offset, bedfh, MPI_UNSIGNED_CHAR, NREADS, bed_data, bytes);
    //@@@MPI_Barrier(MPI_COMM_WORLD);

    check_mpi(MPI_File_close(&bedfh), __LINE__, __FILE__);

    double te = MPI_Wtime();
    MPI_Barrier(MPI_COMM_WORLD);
    if (rank >= 0)
        printf("INFO   : time to load genotype data = %.2f seconds.\n", te - ts);

}

void Bayes::set_block_of_markers() {

    const int modu = Mt % nranks;
    const int size = Mt / nranks;

    Mm = Mt % nranks != 0 ? size + 1 : size;

    int len[nranks], start[nranks];
    int cum = 0;
    for (int i=0; i<nranks; i++) {
        len[i]  = i < modu ? size + 1 : size;
        start[i] = cum;
        cum += len[i];
    }
    //printf("cum %d vs %d  Mm = %d\n", cum, Mt, Mm);
    assert(cum == Mt);

    M = len[rank];
    S = start[rank];

    printf("INFO   : rank %4d has %d markers over tot Mt = %d, max Mm = %d, starting at S = %d\n", rank, M, Mt, Mm, S);
    //@todo: mpi check sum over tasks == Mt
}

void Bayes::print_cva() {
    printf("INFO   : mixtures for all groups:\n");
    for (int i=0; i<G; i++) {
        printf("         grp %2d: ", i);
        for (int j=0; j<K; j++) {
            printf("%7.5f ", cva[i][j]);
        }
        printf("\n");
    }
}

void Bayes::print_cvai() {
    printf("INFO   : inverse mixtures for all groups:\n");
    for (int i=0; i<G; i++) {
        printf("         grp %2d: ", i);
        for (int j=0; j<K; j++) {
            printf("%10.3f ", cvai[i][j]);
        }
        printf("\n");
    }
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/Camera/PinholeModel.h>
#include <vw/Math/EulerAngles.h>

#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
#include <vw/Math/LinearAlgebra.h>
#endif

// For std::setprecision
#include <iomanip>

#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
#include <vw/Camera/TsaiFile.pb.h>
using google::protobuf::RepeatedFieldBackInserter;
#endif

#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

using namespace vw;

// Old deprecated format of Pinhole I/O. Didn't support all distortion options.
// Reads in a file containing parameters of a pinhole model with
// a tsai lens distortion model. An example is provided at the end of this file.
void camera::PinholeModel::read_old_file(std::string const& filename) {

  char line[2048];
  double fu, fv, cu, cv;
  Vector3 u_direction, v_direction, w_direction;
  Vector3 C;
  Matrix3x3 R;
  Vector4 distortion_params(0,0,0,0);

  FILE *cam_file = fopen(filename.c_str(), "r");
  if (cam_file == 0) vw_throw( IOErr() << "PinholeModel::read_file: Could not open file\n" );

  // Read intrinsic parameters
  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"fu = %lf", &fu) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x focal length\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"fv = %lf", &fv) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y focal length\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"cu = %lf", &cu) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x principal point\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"cv = %lf", &cv) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y principal point\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"u_direction = %lf %lf %lf", &u_direction(0), &u_direction(1), &u_direction(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read u direction vector\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"v_direction = %lf %lf %lf", &v_direction(0), &v_direction(1), &v_direction(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read v direction vector\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"w_direction = %lf %lf %lf", &w_direction(0), &w_direction(1), &w_direction(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read w direction vector\n" );
  }

  // Read extrinsic parameters
  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"C = %lf %lf %lf", &C(0), &C(1), &C(2)) != 3) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file: Could not read C (camera center) vector\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if ( sscanf(line, "R = %lf %lf %lf %lf %lf %lf %lf %lf %lf",
              &R(0,0), &R(0,1), &R(0,2),
              &R(1,0), &R(1,1), &R(1,2),
              &R(2,0), &R(2,1), &R(2,2)) != 9 ) {
      fclose(cam_file);
      vw_throw( IOErr() << "PinholeModel::read_file(): Could not read rotation matrix\n" );
  }

  // Read distortion parameters.
   fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"k1 = %lf", &distortion_params[0] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter k1\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"k2 = %lf", &distortion_params[1] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter k2\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"p1 = %lf", &distortion_params[2] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter p1\n" );
  }

  fgets(line, sizeof(line), cam_file);
  if (sscanf(line,"p2 = %lf", &distortion_params[3] ) != 1) {
    fclose(cam_file);
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read tsai distortion parameter p2\n" );
  }

  fclose(cam_file);

  m_u_direction = u_direction;
  m_v_direction = v_direction;
  m_w_direction = w_direction;
  m_pixel_pitch = 1;

  m_fu = fu;
  m_fv = fv;
  m_cu = cu;
  m_cv = cv;
  m_camera_center = C;

  m_rotation = R;
  this->rebuild_camera_matrix();

  if( distortion_params == Vector4(0,0,0,0))
    m_distortion.reset(new NullLensDistortion());
  else
    m_distortion.reset(new TsaiLensDistortion(distortion_params));
}

// Reads in a file containing parameters of a pinhole model with
// a tsai lens distortion model.
void camera::PinholeModel::read_file(std::string const& filename) {
  this->read(filename);
}

void camera::PinholeModel::read(std::string const& filename) {

  fs::path filename_path( filename );
  if ( filename_path.extension() == ".pinhole" ) {
#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
    std::fstream input( filename.c_str(), std::ios::in | std::ios::binary );
    if ( !input )
      vw_throw( IOErr() << "Pinhole::read_file: Could not open " << filename << "\n" );
    TsaiFile file;
    if ( !file.ParseFromIstream( &input ) )
      vw_throw( IOErr() << "Pinhole::read_file: Protocol buffer failed to parse \"" << filename << "\"\n" );
    input.close();

    // Making sure protobuf seems correct
    VW_ASSERT( file.focal_length_size() == 2,
               IOErr() << "Pinhole::read_file: Unexpected amount of focal lengths." );
    VW_ASSERT( file.center_point_size() == 2,
               IOErr() << "Pinhole::read_file: Unexpected amount of center points." );
    VW_ASSERT( file.u_direction_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of u vector." );
    VW_ASSERT( file.v_direction_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of v vector." );
    VW_ASSERT( file.w_direction_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of w vector." );
    VW_ASSERT( file.camera_center_size() == 3,
               IOErr() << "Pinhole::read_file: Unexpected size of camera vector." );
    VW_ASSERT( file.camera_rotation_size() == 9,
               IOErr() << "Pinhole::read_file: Unexpected size of rotation matrix." );

    typedef VectorProxy<double,3> Vector3P;
    m_u_direction = Vector3P(file.mutable_u_direction()->mutable_data());
    m_v_direction = Vector3P(file.mutable_v_direction()->mutable_data());
    m_w_direction = Vector3P(file.mutable_w_direction()->mutable_data());
    m_camera_center = Vector3P(file.mutable_camera_center()->mutable_data());
    m_fu = file.focal_length(0);
    m_fv = file.focal_length(1);
    m_cu = file.center_point(0);
    m_cv = file.center_point(1);
    m_rotation = MatrixProxy<double,3,3>(file.mutable_camera_rotation()->mutable_data());
    m_pixel_pitch = file.pixel_pitch();

    this->rebuild_camera_matrix();

    if ( file.distortion_name() == "NULL" ) {
      VW_ASSERT( file.distortion_vector_size() == 0,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new NullLensDistortion());
    } else if ( file.distortion_name() == "TSAI" ) {
      VW_ASSERT( file.distortion_vector_size() == 4,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new TsaiLensDistortion(VectorProxy<double,4>(file.mutable_distortion_vector()->mutable_data())));
    } else if ( file.distortion_name() == "BROWNCONRADY" ) {
      VW_ASSERT( file.distortion_vector_size() == 8,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new BrownConradyDistortion(VectorProxy<double,8>(file.mutable_distortion_vector()->mutable_data())));
    } else if ( file.distortion_name() == "AdjustableTSAI" ) {
      VW_ASSERT( file.distortion_vector_size() > 3,
                 IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
      m_distortion.reset( new AdjustableTsaiLensDistortion(VectorProxy<double>(file.distortion_vector_size(),file.mutable_distortion_vector()->mutable_data())));
    }
#else
    // If you hit this point, you need to install Google Protobuffers to
    // be in order to write.
    vw_throw( IOErr() << "Pinhole::write_file: Camera IO not supported without Google Protobuffers" );
#endif
  } else if ( filename_path.extension() == ".tsai" ) {
    this->read_old_file( filename );
  } else {
    vw_throw( IOErr() << "Unknown PinholeModel filename extension \""
              << filename_path.extension() << "\"" );
  }
}

// Write parameters of an exiting PinholeModel into a .tsai file for later use.
void camera::PinholeModel::write_file(std::string const& filename) const {
  write(filename);
}
void camera::PinholeModel::write(std::string const& filename) const {
#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
  std::string output_file =
    fs::path(filename).replace_extension(".pinhole").string();

  TsaiFile file;
  file.add_focal_length( m_fu );
  file.add_focal_length( m_fv );
  file.add_center_point( m_cu );
  file.add_center_point( m_cv );
  file.set_pixel_pitch( m_pixel_pitch );

  std::copy(m_u_direction.begin(), m_u_direction.end(),
            RepeatedFieldBackInserter(file.mutable_u_direction()));
  std::copy(m_v_direction.begin(), m_v_direction.end(),
            RepeatedFieldBackInserter(file.mutable_v_direction()));
  std::copy(m_w_direction.begin(), m_w_direction.end(),
            RepeatedFieldBackInserter(file.mutable_w_direction()));
  std::copy(m_camera_center.begin(), m_camera_center.end(),
            RepeatedFieldBackInserter(file.mutable_camera_center()));

  std::copy(m_rotation.begin(), m_rotation.end(),
            RepeatedFieldBackInserter(file.mutable_camera_rotation()));

  file.set_distortion_name( m_distortion->name() );
  Vector<double> distort_vec = m_distortion->distortion_parameters();
  std::copy(distort_vec.begin(),distort_vec.end(),
            RepeatedFieldBackInserter(file.mutable_distortion_vector()));

  std::ofstream output(output_file.c_str());
  if( !output.is_open() )
    vw_throw( IOErr() << "PinholeModel::write_file: Could not open file\n" );
  file.SerializeToOstream( &output );
  output.close();
#else
  // If you hit this point, you need to install Google Protobuffers to
  // be in order to write.
  vw_throw( IOErr() << "Pinhole::write_file: Camera IO not supported without Google Protobuffers" );
#endif
}

Vector2 camera::PinholeModel::point_to_pixel(Vector3 const& point) const {

  //  Multiply the pixel location by the camera matrix.
  double denominator = m_camera_matrix(2,0)*point(0) + m_camera_matrix(2,1)*point(1) +
    m_camera_matrix(2,2)*point(2) + m_camera_matrix(2,3);
  Vector2 pixel = Vector2( (m_camera_matrix(0,0)*point(0) + m_camera_matrix(0,1)*point(1) +
                            m_camera_matrix(0,2)*point(2) + m_camera_matrix(0,3)) / denominator,
                           (m_camera_matrix(1,0)*point(0) + m_camera_matrix(1,1)*point(1) +
                            m_camera_matrix(1,2)*point(2) + m_camera_matrix(1,3)) / denominator);

  //  Apply the lens distortion model
  return m_distortion->distorted_coordinates(*this, pixel)/m_pixel_pitch;
}

bool camera::PinholeModel::projection_valid(Vector3 const& point) const {
  // z coordinate after extrinsic transformation
  double z = m_extrinsics(2, 0)*point(0) + m_extrinsics(2, 1)*point(1) +
    m_extrinsics(2, 2)*point(2) + m_extrinsics(2,3);
  return z > 0;
}

Vector3 camera::PinholeModel::pixel_to_vector (Vector2 const& pix) const {
  // Apply the inverse lens distortion model
  Vector2 undistorted_pix = m_distortion->undistorted_coordinates(*this, pix*m_pixel_pitch);

  // Compute the direction of the ray emanating from the camera center.
  Vector3 p(0,0,1);
  subvector(p,0,2) = undistorted_pix;
  return normalize( m_inv_camera_transform * p);
}

void camera::PinholeModel::intrinsic_parameters(double& f_u, double& f_v,
                                                double& c_u, double& c_v) const {
  f_u = m_fu;  f_v = m_fv;  c_u = m_cu;  c_v = m_cv;
}

void camera::PinholeModel::set_intrinsic_parameters(double f_u, double f_v,
                                                    double c_u, double c_v) {
  m_fu = f_u;  m_fv = f_v;  m_cu = c_u;  m_cv = c_v;
  rebuild_camera_matrix();
}

void camera::PinholeModel::set_camera_matrix( Matrix<double,3,4> const& p ) {
#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
  // Solving for camera center
  Matrix<double> cam_nullsp = nullspace(p);
  Vector<double> cam_center = select_col(cam_nullsp,0);
  cam_center /= cam_center[3];
  m_camera_center = subvector(cam_center,0,3);

  // Solving for intrinsics with RQ decomposition
  Matrix<double> M = submatrix(p,0,0,3,3);
  Matrix<double> R,Q;
  rqd( M, R, Q );
  Matrix<double> sign_fix(3,3);
  sign_fix.set_identity();
  if ( R(0,0) < 0 )
    sign_fix(0,0) = -1;
  if ( R(1,1) < 0 )
    sign_fix(1,1) = -1;
  if ( R(2,2) < 0 )
    sign_fix(2,2) = -1;
  R = R*sign_fix;
  Q = sign_fix*Q;
  R /= R(2,2);

  // Pulling out intrinsic and last extrinsic
  Matrix<double,3,3> uvwRotation;
  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;
  m_rotation = inverse(uvwRotation*Q);
  m_fu = R(0,0);
  m_fv = R(1,1);
  m_cu = R(0,2);
  m_cv = R(1,2);

  if ( fabs(R(0,1)) >= 1.2 )
    vw_out(WarningMessage,"camera") << "Significant skew not modelled by pinhole camera\n";

  // Rebuild
  rebuild_camera_matrix();
#else
  vw_throw( NoImplErr() << "PinholeModel::set_Camera_Matrix is unavailable without LAPACK" );
#endif
}

void camera::PinholeModel::rebuild_camera_matrix() {

  /// The intrinsic portion of the camera matrix is stored as
  ///
  ///    [  fx   0   cx  ]
  /// K= [  0    fy  cy  ]
  ///    [  0    0   1   ]
  ///
  /// with fx, fy the focal length of the system (in horizontal and
  /// vertical pixels), and (cx, cy) the pixel coordinates of the
  /// central pixel (the principal point on the image plane).

  m_intrinsics(0,0) = m_fu;
  m_intrinsics(0,1) = 0;
  m_intrinsics(0,2) = m_cu;
  m_intrinsics(1,0) = 0;
  m_intrinsics(1,1) = m_fv;
  m_intrinsics(1,2) = m_cv;
  m_intrinsics(2,0) = 0;
  m_intrinsics(2,1) = 0;
  m_intrinsics(2,2) = 1;

  // The extrinsics are normally built as the matrix:  [ R | -R*C ].
  // To allow for user-specified coordinate frames, the
  // extrinsics are now build to include the u,v,w rotation
  //
  //               | u_0  u_1  u_2  |
  //     Extr. =   | v_0  v_1  v_2  | * [ R | -R*C]
  //               | w_0  w_1  w_2  |
  //
  // The vectors u,v, and w must be orthonormal.

  /*   check for orthonormality of u,v,w              */
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_v_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_v_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( fabs( norm_2(m_u_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_v_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_w_direction) - 1 ) < 0.001 );

  Matrix<double,3,3> uvwRotation;

  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;

  Matrix<double,3,3> m_rotation_inverse = transpose(m_rotation);
  submatrix(m_extrinsics,0,0,3,3) = uvwRotation * m_rotation_inverse;
  select_col(m_extrinsics,3) = uvwRotation * -m_rotation_inverse * m_camera_center;

  m_camera_matrix = m_intrinsics * m_extrinsics;
  m_inv_camera_transform = inverse(uvwRotation*m_rotation_inverse) * inverse(m_intrinsics);
}

// scale_camera
//  Used to modify camera in the event to user resizes the image
camera::PinholeModel
camera::scale_camera(camera::PinholeModel const& camera_model,
                     float const& scale) {
  Vector2 focal = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  focal *= scale;
  offset *= scale;
  boost::shared_ptr<LensDistortion> lens = camera_model.lens_distortion()->copy();
  lens->scale( scale );
  return camera::PinholeModel( camera_model.camera_center(),
                               camera_model.camera_pose().rotation_matrix(),
                               focal[0], focal[1], offset[0], offset[1],
                               camera_model.coordinate_frame_u_direction(),
                               camera_model.coordinate_frame_v_direction(),
                               camera_model.coordinate_frame_w_direction(),
                               *lens );
}

//   /// Given two pinhole camera models, this method returns two new camera
//   /// models that have been epipolar rectified.
//   template <>
//   void epipolar(PinholeModel<NoLensDistortion> const& src_camera0,
//                 PinholeModel<NoLensDistortion> const& src_camera1,
//                 PinholeModel<NoLensDistortion> &dst_camera0,
//                 PinholeModel<NoLensDistortion> &dst_camera1);
camera::PinholeModel
camera::linearize_camera(camera::PinholeModel const& camera_model) {
  Vector2 focal = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  camera::NullLensDistortion distortion;
  return camera::PinholeModel(camera_model.camera_center(),
      camera_model.camera_pose().rotation_matrix(),
      focal[0], focal[1], offset[0], offset[1],
      camera_model.coordinate_frame_u_direction(),
      camera_model.coordinate_frame_v_direction(),
      camera_model.coordinate_frame_w_direction(),
      distortion);
}

std::ostream& camera::operator<<(std::ostream& str,
                                 camera::PinholeModel const& model) {
  str << "Pinhole camera: \n";
  str << "\tCamera Center: " << model.camera_center() << "\n";
  str << "\tRotation Matrix: " << model.camera_pose() << "\n";
  str << "\tIntrinsics:\n";
  str << "\t  focal: " << model.focal_length() << "\n";
  str << "\t  offset: " << model.point_offset() << "\n";
  str << "\tDistortion Model: " << model.lens_distortion()->name() << "\n";
  str << "\t  " << *model.lens_distortion() << "\n";

  return str;
}


/**
 * @file envelope_collection.cc
 * Computes the reverberation envelope time series for all combinations of
 * receiver azimuth, source beam number, receiver beam number.
 */
#include <usml/eigenverb/envelope_collection.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/foreach.hpp>
#include <netcdfcpp.h>

using namespace usml::eigenverb;

/**
 * Reserve memory in which to store results as a series of
 * nested dynamic arrays.
 */
envelope_collection::envelope_collection(
	const seq_vector* envelope_freq,
	size_t src_freq_first,
	const seq_vector* travel_time,
	double reverb_duration,
	double pulse_length,
	double threshold,
	size_t num_azimuths,
	size_t num_src_beams,
	size_t num_rcv_beams,
	double initial_time,
	sensor_model::id_type source_id,
    sensor_model::id_type receiver_id,
    wposition1 src_position,
    wposition1 rcv_position
) :
	_envelope_freq(envelope_freq->clone()),
	_travel_time( travel_time->clip(0.0,reverb_duration) ),
	_reverb_duration(reverb_duration),
	_pulse_length(pulse_length),
	_threshold( threshold ),
	_num_azimuths(num_azimuths),
	_num_src_beams(num_src_beams),
	_num_rcv_beams(num_rcv_beams),
	_initial_time(initial_time),
	_source_id(source_id),
	_receiver_id(receiver_id),
	_source_position(src_position),
	_receiver_position(rcv_position),
	_envelope_model( _envelope_freq, src_freq_first, _travel_time,
	                        _initial_time, _pulse_length, _threshold)
{
    // Store range from source to receiver when eigenverbs were obtained.
    _slant_range = _receiver_position.distance(_source_position);

	_envelopes = new matrix<double>***[_num_azimuths];
	matrix<double>**** pa = _envelopes;
	for (size_t a = 0; a < _num_azimuths; ++a, ++pa) {
		*pa = new matrix<double>**[_num_src_beams];
		matrix<double>*** ps = *pa;
		for (size_t s = 0; s < _num_src_beams; ++s, ++ps) {
			*ps = new matrix<double>*[_num_rcv_beams];
			matrix<double>** pr = *ps;
			for (size_t r = 0; r < _num_rcv_beams; ++r, ++pr) {
				*pr = new matrix< double >(
						_envelope_freq->size(), _travel_time->size() ) ;
				(*pr)->clear();
			}
		}
	}
}

/**
 * Delete dynamic memory in each of the nested dynamic arrays.
 */
envelope_collection::~envelope_collection() {
	matrix<double>**** pa = _envelopes;
	for (size_t a = 0; a < _num_azimuths; ++a, ++pa) {
		matrix<double>*** ps = *pa;
		for (size_t s = 0; s < _num_src_beams; ++s, ++ps) {
			matrix<double>** pr = *ps;
			for (size_t r = 0; r < _num_rcv_beams; ++r, ++pr) {
				delete *pr ;
			}
			delete[] *ps ;
		}
		delete[] *pa ;
	}
	delete[] _envelopes ;
	delete _envelope_freq ;
	delete _travel_time ;
}

/**
 * Adds the intensity contribution for a single combination of source
 * and receiver eigenverbs.
 */
void envelope_collection::add_contribution(
	const eigenverb& src_verb, const eigenverb& rcv_verb,
	const matrix<double>& src_beam, const matrix<double>& rcv_beam,
	const vector<double>& scatter, double xs2, double ys2 )
{
	size_t azimuth = rcv_verb.az_index ;
	bool ok = _envelope_model.compute_intensity(src_verb,rcv_verb,scatter,xs2,ys2) ;
	if ( ok ) {
		for ( size_t s=0 ; s < src_beam.size2() ; ++s ) {
			for ( size_t r=0 ; r < rcv_beam.size2() ; ++r ) {
				for ( size_t f=0 ; f < _envelope_freq->size() ; ++f ) {
					matrix_row< matrix<double> > intensity(_envelope_model.intensity(), f);
					matrix_row< matrix<double> > envelope(*_envelopes[azimuth][s][r], f);
					envelope += src_beam(f, s) * rcv_beam(f, r) * intensity;
				}
			}
		}
	}
}

/**
 * Updates the envelope_collection data with the parameters provided.
 */
void envelope_collection::dead_reckon(double delta_time,
                                    double slant_range, double prev_range) {
    // Set new slant_range
    _slant_range = slant_range;

    // Shift the time series
    delete _travel_time;
    boost::numeric::ublas::vector<double> temp_data = (*_travel_time);
    temp_data = temp_data + delta_time;
    _travel_time = new seq_data( temp_data );

    { // Scope for lock

        // Perform copy and intensity update
        double gain = slant_range/prev_range;
        gain *= gain ;

        write_lock_guard guard(this->_envelopes_mutex);
        // Copy "this" envelopes to new_collection
        matrix<double>**** pa = _envelopes;
        for (size_t a = 0; a < _num_azimuths; ++a, ++pa)
        {
            matrix<double>*** ps = *pa;
            for (size_t s = 0; s < _num_src_beams; ++s, ++ps)
            {
                matrix<double>** pr = *ps;
                for (size_t r = 0; r < _num_rcv_beams; ++r, ++pr)
                {
                    (**pr) = this->envelope(a, s, r);
                    (**pr) *= gain;
                }
            }
        }
    }
}

/**
 * Writes the envelope data to disk
 */
void envelope_collection::write_netcdf(const char* filename) const {
	NcFile* nc_file = new NcFile(filename, NcFile::Replace);

	// dimensions

	NcDim* azimuth_dim = nc_file->add_dim("azimuth", (long) _num_azimuths ) ;
	NcDim* src_beam_dim = nc_file->add_dim("src_beam", (long) _num_src_beams ) ;
	NcDim* rcv_beam_dim = nc_file->add_dim("rcv_beam", (long) _num_rcv_beams ) ;
	NcDim* freq_dim = nc_file->add_dim("frequency", (long) _envelope_freq->size()) ;
	NcDim* time_dim = nc_file->add_dim("travel_time", (long) _travel_time->size()) ;

	// variables

	NcVar* pulse_length_var = nc_file->add_var("pulse_length", ncDouble );
	NcVar* threshold_var = nc_file->add_var("threshold", ncDouble );
	NcVar* initial_time_var = nc_file->add_var("initial_time", ncDouble );
	NcVar* freq_var = nc_file->add_var("frequency", ncDouble, freq_dim);
	NcVar* time_var = nc_file->add_var("travel_time", ncDouble, time_dim);
	NcVar* envelopes_var = nc_file->add_var("intensity", ncDouble,
		azimuth_dim, src_beam_dim, rcv_beam_dim, freq_dim, time_dim ) ;

	// units

	pulse_length_var->add_att("units", "seconds");
	threshold_var->add_att("units", "dB");
	initial_time_var->add_att("units", "seconds");
	time_var->add_att("units", "seconds");
	freq_var->add_att("units", "hertz");
	envelopes_var->add_att("units", "dB");

	// data

	pulse_length_var->put( &_pulse_length ) ;
	threshold_var->put( &_threshold ) ;
	initial_time_var->put( &_initial_time ) ;
	freq_var->put( _envelope_freq->data().begin(), (long) _envelope_freq->size());
	time_var->put( _travel_time->data().begin(), (long) _travel_time->size());

	for (size_t a = 0; a < _num_azimuths; ++a) {
		for (size_t s = 0; s < _num_src_beams; ++s) {
			for (size_t r = 0; r < _num_rcv_beams; ++r) {
				for (size_t f = 0; f < _envelope_freq->size(); ++f) {
					matrix_row< matrix<double> > row(*_envelopes[a][s][r], f);
					vector<double> envelope = 10.0*log10(max(row, 1e-30));
					envelopes_var->set_cur((long)a, (long)s, (long)r, (long) f, 0L );
					envelopes_var->put(envelope.data().begin(), 1L, 1L, 1L, 1L,
						(long) _travel_time->size() );
				}
			}
		}
	}

    // close file

    delete nc_file; // destructor frees all netCDF temp variables
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 14:40:04

#ifndef MSSMEFTHiggs_mAmu_INPUT_PARAMETERS_H
#define MSSMEFTHiggs_mAmu_INPUT_PARAMETERS_H

#include <complex>
#include <Eigen/Core>

namespace flexiblesusy {

struct MSSMEFTHiggs_mAmu_input_parameters {
   double MSUSY{};
   double M1Input{};
   double M2Input{};
   double M3Input{};
   double MuInput{};
   double mAInput{};
   double TanBeta{};
   Eigen::Matrix<double,3,3> mq2Input{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> mu2Input{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> md2Input{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> ml2Input{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> me2Input{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> AuInput{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> AdInput{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> AeInput{Eigen::Matrix<double,3,3>::Zero()};

   Eigen::ArrayXd get() const;
   void set(const Eigen::ArrayXd&);
};

std::ostream& operator<<(std::ostream&, const MSSMEFTHiggs_mAmu_input_parameters&);

} // namespace flexiblesusy

#endif