/* Simulates independent realizations of a homogeneous SIR process on
a temporal network given on the form: (t i j) with one triple per line
and t,i, and j separated by tabs. ("\t").

Should be compiled with g++ including the boost library and using the
option -O2, i.e., as
g++ SIR-Poisson-homogeneous.cpp -o SIR -O2 -I<path of boost>

With the program compiled as SIR, it is called from the shell as:
./SIR <data> dt beta mu T_simulation ensembleSize outputTimeResolution
where:
<data> - path of text file containing contact data (temporal network);
dt - time-resolution of recorded contact data (time-step length);
beta - probability per time-step of infection when in contact with an
    infected node;
mu - probability per time-step of recovery for an infected node;
T_simulation - length of simulation in number of time-steps;
ensembleSize - number of independent realizations of the SIR process;
outputTimeResolution - time-resolution of the average number of infected
    and recovered nodes that the program gives as output.

The program gives as output three text files containing the average
number of infected nodes as function of time, the average number of
recovered nodes as function of time, and a histogram of the number of
recovered nodes at t=T_simulation-1.*/
//======================================================================
// Libraries
//======================================================================
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <algorithm> //for sort and unique
#include <vector>
#include <iterator>
#include <ctime>

//======================================================================
// Typedef
//======================================================================
// Variable types:
typedef unsigned int COUNTER;
typedef unsigned int NODE;
typedef std::vector<NODE> NODES; //list of nodes
typedef std::vector<bool> BOOLS;
struct CONTACT {NODE i; NODE j;}; //contact (i,j)
typedef std::vector<CONTACT> CONTACTS; //list of contacts during a single time-frame
typedef std::vector<CONTACTS> CONTACTS_LIST; //list of contact lists
// Random number generators:
typedef boost::mt19937_64 ENG; //use Mersenne Twister 19937 as PRNG engine
typedef boost::uniform_int<> DIST_INT; //define uniform distribution of integers
typedef boost::uniform_real<> DIST_REAL; //define uniform distribution of reals on [0,1)
typedef boost::exponential_distribution<> DIST_EXP; //define exponential distribution

//======================================================================
// Global parameters
//======================================================================
COUNTER N; //number of nodes in network
COUNTER T_data; //length of dataset (time steps)
COUNTER dt; //input time resolution (integer)
char inputname[200], outputname[200];
// Input/Output streams:
std::ofstream output;
std::ifstream input;
// Initialize PRNG:
ENG eng(9071982);
DIST_INT dist_int;
DIST_REAL dist_rand(0,1);
DIST_EXP dist_exp;

//======================================================================
// Function for importing list of contacts from 't\ti\tj\n' format file:
//======================================================================
CONTACTS_LIST loadContactListList(char *inputname)
{
    // Timer:
    std::clock_t clockStart = std::clock();

    // Define list of contact lists and list of nodes, etc.:
    std::string line;
    COUNTER t;
    NODE i,j;
    NODES nodes;
    CONTACTS_LIST contactListList;
    CONTACTS contactList;
    CONTACT contact;
    NODES::iterator last;

    // Print name of input file to screen:
    std::cout << "Filename: " << inputname << std::endl;

    input.open(inputname);
    // If input-file is not found or cannot be read, raise error and exit:
    if(input==NULL){ std::cout << "ERROR! File cannot be read."; return contactListList; }

    // Create list of nodes in file:
    while(getline(input,line))
    {
        input>>t>>i>>j;
        nodes.push_back(i);
        nodes.push_back(j);
    }
    T_data=t+1; //length of dataset
    std::cout << "T=" << T_data << std::endl;
    input.close();
    // Sort list and remove duplicates:
    std::sort(nodes.begin(),nodes.end());
    last=unique(nodes.begin(),nodes.end());
    nodes.resize(distance(nodes.begin(),last));
    N=nodes.size(); //number of unique nodes (network size)
    // List for redefining node IDs:
    NODES nodeIDs(nodes[nodes.size()-1]+1);
    std::cout << std::endl;
    for(COUNTER n=0; n<N; n++)
    {
        nodeIDs[nodes[n]]=n;
    }

    // Read first line of inputfile and get t,i,j:
    input.open(inputname);
    getline(input,line);
    input>>t>>i>>j;
    // Loop over t<T and create list of contact lists:
    for(COUNTER tt=0; tt<T_data; tt+=dt)
    {
        while(t==tt && !input.eof())
        {
            contact.i=nodeIDs[i]; contact.j=nodeIDs[j];
            contactList.push_back(contact);
            // Read line and get t,i,j:
            getline(input,line);
            input>>t>>i>>j;
        }
        contactListList.push_back(contactList);
        contactList.clear();
    }
    input.close();

    std::cout << std::endl << N << " nodes. Construction time: " << ( clock() - clockStart ) / (double) CLOCKS_PER_SEC << " s\n\n";

    return contactListList;
}

//======================================================================
// Main:
//======================================================================
int main(int argc, char *argv[])
{
    //-------------------------------------------------------------------------------------
    // Load contact data set:
    //-------------------------------------------------------------------------------------
    // Check if correct number of parameters was passed to program:
    // Check if correct number of parameters was passed to program:
    if(argc<3){ std::cout << "Error! Data file not defined.\n"; return 0; }
    else
    {
        if(argc<5){ std::cout << "Error! Epidemic parameters not defined.\n"; return 0; }
        else
        {
            if(argc<6){ std::cout << "Error! Simulation time not specified.\n"; return 0; }
            else
            {
                if(argc<7){ std::cout << "Error! Ensemble size not specified.\n"; return 0; }
                else
                {
                    if(argc<8){ std::cout << "Error! Output time-resolution not specified.\n"; return 0;}
                }
            }
        }
    }
    // Set parameter values as specified:
    char *datafile=argv[1]; //dataset file name
    dt=atoi(argv[2]); //dataset time resolution (integer)
    char *beta_str = argv[3]; // base infection rate
    double beta = atof(beta_str);
    char *mu_str = argv[4]; // base recovery rate
    double mu = atof(mu_str);
    COUNTER betaprec=strlen(beta_str)-2;
    COUNTER muprec=strlen(mu_str)-2;
    COUNTER T_simulation = atoi(argv[5]); //simulation time
    COUNTER ensembleSize = atoi(argv[6]); //ensemble size (number of realizations)
    COUNTER outputTimeResolution = atoi(argv[7]); //output time-resolution

    // Open input file and load contact_lists:
    sprintf(inputname,"%s",datafile);
    const CONTACTS_LIST contactListList_original=loadContactListList(inputname);
    // Check if length of contacts_list > 0 and end program if it is not:
    if(contactListList_original.size()==0){ std::cout << "Error! Dataset empty.\n"; return 0; }

    //-------------------------------------------------------------------------------------
    // Define variables:
    //-------------------------------------------------------------------------------------
    NODES infected; //list of infected nodes
    double Mu; //cumulative recovery rate
    BOOLS isSusceptible, isInfected; //list which nodes are susceptible/infected, respectively
    COUNTER I,R; //number of infected and recovered nodes
    COUNTER SI; //number of susceptible nodes in contact with infectious nodes
    NODES si_s; //list of susceptible nodes in contact with infected nodes
    double Beta; //cumulative infection rate
    double Lambda; //cumulative transition rate
    double xi; //
    COUNTER t; //time counter
    COUNTER t_infectionStart; //starting time of infection
    NODE root; //root node of infection
    NODE i,j;
    double tau; //renormalized waiting time until next event
    CONTACTS_LIST contactListList(contactListList_original.size());
    CONTACTS_LIST::iterator contactList_iterator; //iterator over list of contacts
    CONTACTS::iterator contact_iterator; //iterator over contacts
    double r_transitionType; //random variable for choosing which type of transition happens
    COUNTER m; //transition process number
    COUNTER n; //time-counter
    NODES::iterator node_iterator; //iterator over list of nodes
    NODES::reverse_iterator node_reverseIterator; //reverse iterator over list of nodes
    NODES::iterator last; //iterator for use when generating unique list of new infected nodes
    NODES numbersToRemove;
    // Containers for output data:
    NODES sumI_t(T_simulation/outputTimeResolution); //list of number of infected nodes in each recorded frame
    NODES sumR_t(T_simulation/outputTimeResolution); //list of number of recovered nodes in each recorded frame
    NODES hist_R(N+1); //histogram of R values after I=0
    // Random number generators:
    boost::variate_generator<ENG,DIST_INT> randint(eng,dist_int); //random integer
    boost::variate_generator<ENG,DIST_REAL> rand(eng,dist_rand); //random float on [0,1[
    boost::variate_generator<ENG,DIST_EXP> randexp(eng,dist_exp); //random exponentially distributed float

    //-------------------------------------------------------------------------------------
    // Simulate:
    //-------------------------------------------------------------------------------------
    std::clock_t clockStart = std::clock();     //timer
    COUNTER stopped=0; //counter of number of simulations that stopped (I=0) during T_simulation
    for(COUNTER q=0; q<ensembleSize; q++)
    {
        std::cout << q << "/" << ensembleSize << std::endl; //print realization # to screen
        // Copy list of contacts:
        contactListList=contactListList_original;
        // Choose at random infectious root node:
        root=randint(N);
        // Initialize lists of infected nodes and infected node IDs:
        R=0;
        infected.clear();
        infected.push_back(root);
        I=1;
        Mu=mu;
        isInfected.assign(N,false);
        isInfected[root]=true;
        isSusceptible.assign(N,true);
        isSusceptible[root]=false;
        // First waiting "time":
        tau=randexp(1);
        // Random starting time of infection:
        t_infectionStart=randint(T_data);
        // Reset simulation time to zero:
        t=0;

        // Loop until either I=0 or t>=T_simulation
        while(I>0 && t<T_simulation)
        {
            // Loop over list of contact lists:
            for(contactList_iterator=contactListList.begin()+t_infectionStart; contactList_iterator!=contactListList.end(); contactList_iterator++)
            {
                // Create list of susceptible nodes in contact with infected nodes:
                si_s.clear();
                numbersToRemove.clear();
                n=0;
                for(contact_iterator=(*contactList_iterator).begin(); contact_iterator!=(*contactList_iterator).end(); contact_iterator++, n++)
                {
                    i=(*contact_iterator).i;
                    j=(*contact_iterator).j;
                    if(isSusceptible[i])
                    {
                        if(isInfected[j]){ si_s.push_back(i); }
                        else
                        {
                            if(!isSusceptible[j]){ numbersToRemove.push_back(n); }
                        }
                    }
                    else
                    {
                        if(isSusceptible[j])
                        {
                            if(isInfected[i]){ si_s.push_back(j); }
                            else{ numbersToRemove.push_back(n); }
                        }
                        else{ numbersToRemove.push_back(n); }
                    }
                }
                // Remove obsolete contacts:
                for(node_reverseIterator=numbersToRemove.rbegin(); node_reverseIterator!=numbersToRemove.rend(); node_reverseIterator++)
                {
//                    std::cout << *node_reverseIterator << " ";
                    (*contactList_iterator)[*node_reverseIterator]=(*contactList_iterator).back();
                    (*contactList_iterator).pop_back();
                }
                SI=si_s.size(); //number of possible S->I transitions
                Beta=(double)SI*beta; //cumulative infection rate
                Lambda=Beta+Mu; //cumulative transition rate:

                // Check if transition takes place during time-step:
                if(tau>=Lambda) //no transition takes place
                {
                    tau-=Lambda;
                }
                else //at least one transition takes place
                {
                    xi=1.; //fraction of time-step left before transition
                    // Sampling step:
                    while(tau<xi*Lambda) //repeat if next tau is smaller than ~ Lambda-tau
                    {
                        xi-=tau/Lambda; //fraction of time-step left after transition
                        r_transitionType=Lambda*rand(); //random variable for weighted sampling of transitions
                        if(r_transitionType<Beta) //S->I
                        {
                            m=randint(SI); //transition m
                            isInfected[si_s[m]]=true;
                            isSusceptible[si_s[m]]=false;
                            // Add infected node to list:
                            infected.push_back(si_s[m]);
                            I++;
                            Mu+=mu;
                        }
                        else //I->R
                        {
                            m=randint(I); //transition m
                            isInfected[infected[m]]=false;
                            // Remove drawn element from infected:
                            infected[m]=infected.back();
                            infected.pop_back();
                            I--;
                            R++;
                            Mu-=mu;
                        }
                        // Redo list of S-I contacts:
                        si_s.clear();
                        for(contact_iterator=(*contactList_iterator).begin(); contact_iterator!=(*contactList_iterator).end(); contact_iterator++)
                        {
                           i=(*contact_iterator).i;
                           j=(*contact_iterator).j;
                           if(isInfected[i])
                           {
                                if(isSusceptible[j])
                                {
                                    si_s.push_back(j);
                                }
                            }
                            else
                            {
                                if(isInfected[j])
                                {
                                    if(isSusceptible[i])
                                    {
                                        si_s.push_back(i);
                                    }
                                }
                            }
                        }
                        SI=si_s.size();
                        Beta=beta*(double)SI;
                        Lambda=Beta+Mu; //new cumulative transition rate
                        // Draw new renormalized waiting time:
                        tau=randexp(1);
                    }
                }
                // Stop if I=0:
                if(I==0)
                {
                    stopped++;
                    hist_R[R]++;
                    for(n=t; n<T_simulation; n++)
                    {
                        if(n % outputTimeResolution ==0){ sumR_t[n/outputTimeResolution]+=R; }
                    }
                   break;
                }
                // Read out I and R if t is divisible by outputTimeResolution
                if(t % outputTimeResolution ==0)
                {
                    if(t>=T_simulation) //stop if max simulation time-steps has been reached
                    {
                        break;
                    }
                    else
                    {
                        sumI_t[t/outputTimeResolution]+=I;
                        sumR_t[t/outputTimeResolution]+=R;
                    }
                }
                t++;
            }
            t_infectionStart=0;
        }
    }
    double t_simu = ( std::clock() - clockStart ) / (double) CLOCKS_PER_SEC;

    //-------------------------------------------------------------------------------------
    // Save epidemic data to disk:
    //-------------------------------------------------------------------------------------
    clockStart=std::clock();
    // Open output file:
    sprintf(outputname,"avg(I_t)-%s,N=%u,dt=%u,T=%u,beta=%.*f,mu=%.*f,Q=%u,res=%u.txt",datafile,N,dt,T_simulation,betaprec,beta,muprec,mu,ensembleSize,outputTimeResolution);
    output.open(outputname);
    // Write I_t to file:
    for(node_iterator=sumI_t.begin(); node_iterator!=sumI_t.end(); node_iterator++)
    {
        output << (double)*node_iterator/(double)ensembleSize << "\t";
    }
    output.close();

    // Open output file:
    sprintf(outputname,"avg(R_t)-%s,N=%u,dt=%u,T=%u,beta=%.*f,mu=%.*f,Q=%u,res=%u.txt",datafile,N,dt,T_simulation,betaprec,beta,muprec,mu,ensembleSize,outputTimeResolution);
    output.open(outputname);
    // Write R_t to file:
    for(node_iterator=sumR_t.begin(); node_iterator!=sumR_t.end(); node_iterator++)
    {
        output << (double)*node_iterator/(double)ensembleSize << "\t";
    }
    output << "\n";
    output.close();
    // Open output file:
    sprintf(outputname,"p(R)-%s,N=%u,dt=%u,T=%u,beta=%.*f,mu=%.*f,Q=%u,res=%u.txt",datafile,N,dt,T_simulation,betaprec,beta,muprec,mu,ensembleSize,outputTimeResolution);
    output.open(outputname);
    // Write histrogram of R, h(R) to file:
    for(node_iterator=hist_R.begin(); node_iterator!=hist_R.end(); node_iterator++)
    {
        output << (double)*node_iterator/(double)ensembleSize << "\t";
    }
    output.close();
    double t_write = ( std::clock() - clockStart ) / (double) CLOCKS_PER_SEC;

    std::cout << std::endl << "temporal Gillespie---homogeneous & Poissonian SIR w/ contact removal: N=" << N
              << ", T=" << T_data << ", beta=" << beta << ", mu=" << mu;
    std::cout << ", output time-resolution = " << outputTimeResolution << std::endl;
    std::cout << "Simulation time: " << t_simu << "s, Stopped simulations: " << stopped << "/" << ensembleSize << std::endl;
    std::cout << "Writing to file: " << t_write << "s" << std::endl;

    return 0;
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#ifndef __itkDwiPhantomGenerationFilter_cpp
#define __itkDwiPhantomGenerationFilter_cpp

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

#include "itkDwiPhantomGenerationFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <itkOrientationDistributionFunction.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>

#include <boost/progress.hpp>

namespace itk {

  template< class TOutputScalarType >
  DwiPhantomGenerationFilter< TOutputScalarType >
  ::DwiPhantomGenerationFilter()
    : m_BValue(1000)
    , m_SignalScale(1000)
    , m_BaselineImages(0)
    , m_MaxBaseline(0)
    , m_MeanBaseline(0)
    , m_NoiseVariance(0.004)
    , m_AddNoiseFlag(true) // parameters.m_Misc.m_CheckAddNoiseBox
    , m_GreyMatterAdc(0.01)
    , m_SimulateBaseline(true)
    , m_DefaultBaseline(1000)
  {
    this->SetNumberOfRequiredOutputs (1);
    m_Spacing.Fill(2.5); m_Origin.Fill(0.0);
    m_DirectionMatrix.SetIdentity();
    m_ImageRegion.SetSize(0, 10);
    m_ImageRegion.SetSize(1, 10);
    m_ImageRegion.SetSize(2, 10);

    typename OutputImageType::Pointer outImage = OutputImageType::New();

    outImage->SetSpacing( m_Spacing );   // Set the image spacing
    outImage->SetOrigin( m_Origin );     // Set the image origin
    outImage->SetDirection( m_DirectionMatrix );  // Set the image direction
    outImage->SetLargestPossibleRegion( m_ImageRegion );
    outImage->SetBufferedRegion( m_ImageRegion );
    outImage->SetRequestedRegion( m_ImageRegion );
    outImage->SetVectorLength(QBALL_ODFSIZE);
    outImage->Allocate();
    // ITKv4 migration fix : removing OutputImageType::PixelType(0.0)
    // the conversion is handled internally by the itk::Image
    typename OutputImageType::PixelType fillValue(0.0);
    outImage->FillBuffer( fillValue );

    this->SetNthOutput (0, outImage);

  }

  template< class TOutputScalarType >
  void DwiPhantomGenerationFilter< TOutputScalarType >
  ::GenerateTensors()
  {
    MITK_INFO << "Generating tensors";

    for (int i=0; i<m_SignalRegions.size(); i++)
    {
      float ADC = m_TensorADC.at(i);
      float FA = m_TensorFA.at(i);
      itk::DiffusionTensor3D<float> kernel;
      kernel.Fill(0);
      float e1=ADC*(1+2*FA/sqrt(3-2*FA*FA));
      float e2=ADC*(1-FA/sqrt(3-2*FA*FA));
      float e3=e2;
      kernel.SetElement(0,e1);
      kernel.SetElement(3,e2);
      kernel.SetElement(5,e3);

      if (m_SimulateBaseline)
      {
        double l2 = GetTensorL2Norm(kernel);
        if (l2>m_MaxBaseline)
          m_MaxBaseline = l2;
      }

      MITK_INFO << "Kernel FA: " << kernel.GetFractionalAnisotropy();
      vnl_vector_fixed<double, 3> kernelDir; kernelDir[0]=1; kernelDir[1]=0; kernelDir[2]=0;

      itk::DiffusionTensor3D<float> tensor;
      vnl_vector_fixed<double, 3> dir = m_TensorDirection.at(i);
      MITK_INFO << "Tensor direction: " << dir;

      dir.normalize();

      vnl_vector_fixed<double, 3> axis = vnl_cross_3d(kernelDir, dir); axis.normalize();
      vnl_quaternion<double> rotation(axis, acos(dot_product(kernelDir, dir)));
      rotation.normalize();
      vnl_matrix_fixed<double, 3, 3> matrix = rotation.rotation_matrix_transpose();

      vnl_matrix_fixed<double, 3, 3> tensorMatrix;
      tensorMatrix[0][0] = kernel[0]; tensorMatrix[0][1] = kernel[1]; tensorMatrix[0][2] = kernel[2];
      tensorMatrix[1][0] = kernel[1]; tensorMatrix[1][1] = kernel[3]; tensorMatrix[1][2] = kernel[4];
      tensorMatrix[2][0] = kernel[2]; tensorMatrix[2][1] = kernel[4]; tensorMatrix[2][2] = kernel[5];

      tensorMatrix = matrix.transpose()*tensorMatrix*matrix;
      tensor[0] = tensorMatrix[0][0]; tensor[1] = tensorMatrix[0][1]; tensor[2] = tensorMatrix[0][2];
      tensor[3] = tensorMatrix[1][1]; tensor[4] = tensorMatrix[1][2]; tensor[5] = tensorMatrix[2][2];

      m_TensorList.push_back(tensor);
    }
  }

  template< class TOutputScalarType >
  void DwiPhantomGenerationFilter< TOutputScalarType >::AddNoise(typename OutputImageType::PixelType& pix)
  {
    for( unsigned int i=0; i<m_GradientList.size(); i++)
    {
      float signal = pix[i];
      float val = sqrt(pow(signal + m_SignalScale*m_RandGen->GetNormalVariate(0.0, m_NoiseVariance), 2) +  pow(m_SignalScale*m_RandGen->GetNormalVariate(0.0, m_NoiseVariance),2));
      pix[i] += val;
    }
  }

  template< class TOutputScalarType >
  typename DwiPhantomGenerationFilter< TOutputScalarType >::OutputImageType::PixelType
  DwiPhantomGenerationFilter< TOutputScalarType >::SimulateMeasurement(itk::DiffusionTensor3D<float>& T, float weight)
  {
    typename OutputImageType::PixelType out;
    out.SetSize(m_GradientList.size());
    out.Fill(0);

    TOutputScalarType s0 = m_DefaultBaseline;
    if (m_SimulateBaseline)
      s0 = (GetTensorL2Norm(T)/m_MaxBaseline)*m_SignalScale;

    for( unsigned int i=0; i<m_GradientList.size(); i++)
    {
      GradientType g = m_GradientList[i];

      if (g.GetNorm()>0.0001)
      {
        itk::DiffusionTensor3D<float> S;
        S[0] = g[0]*g[0];
        S[1] = g[1]*g[0];
        S[2] = g[2]*g[0];
        S[3] = g[1]*g[1];
        S[4] = g[2]*g[1];
        S[5] = g[2]*g[2];

        double D = T[0]*S[0] + T[1]*S[1] + T[2]*S[2] +
            T[1]*S[1] + T[3]*S[3] + T[4]*S[4] +
            T[2]*S[2] + T[4]*S[4] + T[5]*S[5];

        // check for corrupted tensor and generate signal
        if (D>=0)
        {
          D = weight*s0*exp ( -m_BValue * D );
          out[i] = static_cast<TOutputScalarType>( D );
        }
      }
      else
        out[i] = s0;
    }

    return out;
  }

  template< class TOutputScalarType >
  void DwiPhantomGenerationFilter< TOutputScalarType >
  ::GenerateData()
  {
    if (m_NoiseVariance < 0 && m_AddNoiseFlag)
    {
      m_NoiseVariance = 0.001;
    }

    if (!m_SimulateBaseline)
    {
      MITK_INFO << "Baseline image values are set to default. Noise variance value is treated as SNR!";
      if (m_NoiseVariance <= 0 && m_AddNoiseFlag)
      {
        m_NoiseVariance = 0.0001;
      }
      else if (m_NoiseVariance>99 && m_AddNoiseFlag)
      {
        m_NoiseVariance = 0;
      }
      else
      {
        m_NoiseVariance = m_DefaultBaseline/(m_NoiseVariance*m_SignalScale);
        m_NoiseVariance *= m_NoiseVariance;
      }
    }

    m_RandGen = Statistics::MersenneTwisterRandomVariateGenerator::New();
    m_RandGen->SetSeed();

    typename OutputImageType::Pointer outImage = OutputImageType::New();
    outImage->SetSpacing( m_Spacing );
    outImage->SetOrigin( m_Origin );
    outImage->SetDirection( m_DirectionMatrix );
    outImage->SetLargestPossibleRegion( m_ImageRegion );
    outImage->SetBufferedRegion( m_ImageRegion );
    outImage->SetRequestedRegion( m_ImageRegion );
    outImage->SetVectorLength(m_GradientList.size());
    outImage->Allocate();
    typename OutputImageType::PixelType pix;
    pix.SetSize(m_GradientList.size());
    pix.Fill(0.0);
    outImage->FillBuffer(pix);
    this->SetNthOutput (0, outImage);

    double minSpacing = m_Spacing[0];
    if (m_Spacing[1]<minSpacing)
      minSpacing = m_Spacing[1];
    if (m_Spacing[2]<minSpacing)
      minSpacing = m_Spacing[2];

    m_DirectionImageContainer = ItkDirectionImageContainer::New();
    for (int i=0; i<m_SignalRegions.size(); i++)
    {
      itk::Vector< float, 3 > nullVec; nullVec.Fill(0.0);
      ItkDirectionImage::Pointer img = ItkDirectionImage::New();
      img->SetSpacing( m_Spacing );
      img->SetOrigin( m_Origin );
      img->SetDirection( m_DirectionMatrix );
      img->SetRegions( m_ImageRegion );
      img->Allocate();
      img->FillBuffer(nullVec);
      m_DirectionImageContainer->InsertElement(m_DirectionImageContainer->Size(), img);
    }
    m_NumDirectionsImage = ItkUcharImgType::New();
    m_NumDirectionsImage->SetSpacing( m_Spacing );
    m_NumDirectionsImage->SetOrigin( m_Origin );
    m_NumDirectionsImage->SetDirection( m_DirectionMatrix );
    m_NumDirectionsImage->SetRegions( m_ImageRegion );
    m_NumDirectionsImage->Allocate();
    m_NumDirectionsImage->FillBuffer(0);

    m_SNRImage = ItkFloatImgType::New();
    m_SNRImage->SetSpacing( m_Spacing );
    m_SNRImage->SetOrigin( m_Origin );
    m_SNRImage->SetDirection( m_DirectionMatrix );
    m_SNRImage->SetRegions( m_ImageRegion );
    m_SNRImage->Allocate();
    m_SNRImage->FillBuffer(0);

    vtkSmartPointer<vtkCellArray> m_VtkCellArray = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPoints>    m_VtkPoints = vtkSmartPointer<vtkPoints>::New();

    m_BaselineImages = 0;
    for( unsigned int i=0; i<m_GradientList.size(); i++)
      if (m_GradientList[i].GetNorm()<=0.0001)
        m_BaselineImages++;

    typedef ImageRegionIterator<OutputImageType>      IteratorOutputType;
    IteratorOutputType it (outImage, m_ImageRegion);

    // isotropic tensor
    itk::DiffusionTensor3D<float> isoTensor;
    isoTensor.Fill(0);
    float e1 = m_GreyMatterAdc;
    float e2 = m_GreyMatterAdc;
    float e3 = m_GreyMatterAdc;
    isoTensor.SetElement(0,e1);
    isoTensor.SetElement(3,e2);
    isoTensor.SetElement(5,e3);
    m_MaxBaseline = GetTensorL2Norm(isoTensor);

    GenerateTensors();

    // simulate measurement
    m_MeanBaseline = 0;
    double noiseStdev = sqrt(m_NoiseVariance);
    while(!it.IsAtEnd())
    {
      pix = it.Get();
      typename OutputImageType::IndexType index = it.GetIndex();

      int numDirs = 0;
      for (int i=0; i<m_SignalRegions.size(); i++)
      {
        ItkUcharImgType::Pointer region = m_SignalRegions.at(i);

        if (region->GetPixel(index)!=0)
        {
          numDirs++;
          pix += SimulateMeasurement(m_TensorList[i], m_TensorWeight[i]);

          // set direction image pixel
          ItkDirectionImage::Pointer img = m_DirectionImageContainer->GetElement(i);
          itk::Vector< float, 3 > pixel = img->GetPixel(index);
          vnl_vector_fixed<double, 3> dir = m_TensorDirection.at(i);
          dir.normalize();
          dir *= m_TensorWeight.at(i);
          pixel.SetElement(0, dir[0]);
          pixel.SetElement(1, dir[1]);
          pixel.SetElement(2, dir[2]);
          img->SetPixel(index, pixel);

          vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
          itk::ContinuousIndex<double, 3> center;
          center[0] = index[0];
          center[1] = index[1];
          center[2] = index[2];
          itk::Point<double> worldCenter;
          outImage->TransformContinuousIndexToPhysicalPoint( center, worldCenter );
          itk::Point<double> worldStart;
          worldStart[0] = worldCenter[0]-dir[0]/2 * minSpacing;
          worldStart[1] = worldCenter[1]-dir[1]/2 * minSpacing;
          worldStart[2] = worldCenter[2]-dir[2]/2 * minSpacing;
          vtkIdType id = m_VtkPoints->InsertNextPoint(worldStart.GetDataPointer());
          container->GetPointIds()->InsertNextId(id);
          itk::Point<double> worldEnd;
          worldEnd[0] = worldCenter[0]+dir[0]/2 * minSpacing;
          worldEnd[1] = worldCenter[1]+dir[1]/2 * minSpacing;
          worldEnd[2] = worldCenter[2]+dir[2]/2 * minSpacing;
          id = m_VtkPoints->InsertNextPoint(worldEnd.GetDataPointer());
          container->GetPointIds()->InsertNextId(id);
          m_VtkCellArray->InsertNextCell(container);
        }
      }

      if (numDirs>1)
      {
        for (int i=0; i<m_GradientList.size(); i++) { pix[i] /= numDirs; }

      }
      else if (numDirs==0)
      {
        if (m_SimulateBaseline) { pix = SimulateMeasurement(isoTensor, 1.0); }

        else { pix.Fill(0.0); }

      }

      m_MeanBaseline += pix[0];
      it.Set(pix);
      m_NumDirectionsImage->SetPixel(index, numDirs);
      if (m_NoiseVariance>0 && m_AddNoiseFlag)
      {
        m_SNRImage->SetPixel(index, pix[0]/(noiseStdev*m_SignalScale));
      }
      ++it;
    }
    m_MeanBaseline /= m_ImageRegion.GetNumberOfPixels();
    if (m_NoiseVariance>0 && m_AddNoiseFlag)
    {
      MITK_INFO << "Mean SNR: " << m_MeanBaseline/(noiseStdev*m_SignalScale);
    }
    else
    {
      MITK_INFO << "No noise added";
    }

    // add rician noise
    it.GoToBegin();
    while(!it.IsAtEnd())
    {
      pix = it.Get();
      AddNoise(pix);
      it.Set(pix);
      ++it;
    }

    // generate fiber bundle
    vtkSmartPointer<vtkPolyData> directionsPolyData = vtkSmartPointer<vtkPolyData>::New();
    directionsPolyData->SetPoints(m_VtkPoints);
    directionsPolyData->SetLines(m_VtkCellArray);
    m_OutputFiberBundle = mitk::FiberBundle::New(directionsPolyData);
  }
  template< class TOutputScalarType >
  double DwiPhantomGenerationFilter< TOutputScalarType >::GetTensorL2Norm(itk::DiffusionTensor3D<float>& T)
  {
    return sqrt(T[0]*T[0] + T[3]*T[3] + T[5]*T[5] + T[1]*T[2]*2.0 + T[2]*T[4]*2.0 + T[1]*T[4]*2.0);
  }

}

#endif // __itkDwiPhantomGenerationFilter_cpp


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Distributed under 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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_SPARSE_
#define _BOOST_UBLAS_OPERATION_SPARSE_

#include <boost/numeric/ublas/traits.hpp>

// These scaled additions were borrowed from MTL unashamedly.
// But Alexei Novakov had a lot of ideas to improve these. Thanks.

namespace boost { namespace numeric { namespace ublas {

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI,
                 row_major_tag) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

        // ISSUE why is there a dense vector here?
        vector<value_type> temporary (e2 ().size2 ());
        temporary.clear ();
        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
            size_type jb (temporary.size ());
            size_type je (0);
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                // temporary.plus_assign (*it2 * row (e2 (), it2.index2 ()));
                matrix_row<expression2_type> mr (e2 (), it2.index2 ());
                typename matrix_row<expression2_type>::const_iterator itr (mr.begin ());
                typename matrix_row<expression2_type>::const_iterator itr_end (mr.end ());
                while (itr != itr_end) {
                    size_type j (itr.index ());
                    temporary (j) += *it2 * *itr;
                    jb = (std::min) (jb, j);
                    je = (std::max) (je, j);
                    ++ itr;
                }
                ++ it2;
            }
            for (size_type j = jb; j < je + 1; ++ j) {
                if (temporary (j) != value_type/*zero*/()) {
                    // FIXME we'll need to extend the container interface!
                    // m.push_back (it1.index1 (), j, temporary (j));
                    // FIXME What to do with adaptors?
                    // m.insert (it1.index1 (), j, temporary (j));
                    if (triangular_restriction::other (it1.index1 (), j))
                        m (it1.index1 (), j) = temporary (j);
                    temporary (j) = value_type/*zero*/();
                }
            }
            ++ it1;
        }
        return m;
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI,
                 column_major_tag) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

        // ISSUE why is there a dense vector here?
        vector<value_type> temporary (e1 ().size1 ());
        temporary.clear ();
        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
            size_type ib (temporary.size ());
            size_type ie (0);
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                // column (m, it2.index2 ()).plus_assign (*it1 * column (e1 (), it1.index1 ()));
                matrix_column<expression1_type> mc (e1 (), it1.index1 ());
                typename matrix_column<expression1_type>::const_iterator itc (mc.begin ());
                typename matrix_column<expression1_type>::const_iterator itc_end (mc.end ());
                while (itc != itc_end) {
                    size_type i (itc.index ());
                    temporary (i) += *it1 * *itc;
                    ib = (std::min) (ib, i);
                    ie = (std::max) (ie, i);
                    ++ itc;
                }
                ++ it1;
            }
            for (size_type i = ib; i < ie + 1; ++ i) {
                if (temporary (i) != value_type/*zero*/()) {
                    // FIXME we'll need to extend the container interface!
                    // m.push_back (i, it2.index2 (), temporary (i));
                    // FIXME What to do with adaptors?
                    // m.insert (i, it2.index2 (), temporary (i));
                    if (triangular_restriction::other (i, it2.index2 ()))
                        m (i, it2.index2 ()) = temporary (i);
                    temporary (i) = value_type/*zero*/();
                }
            }
            ++ it2;
        }
        return m;
    }

    // Dispatcher
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, TRI, bool init = true) {
        typedef typename M::value_type value_type;
        typedef TRI triangular_restriction;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return sparse_prod (e1, e2, m, triangular_restriction (), orientation_category ());
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 TRI) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        // FIXME needed for c_matrix?!
        // return sparse_prod (e1, e2, m, triangular_restriction (), false);
        return sparse_prod (e1, e2, m, triangular_restriction (), true);
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2,
                 M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return sparse_prod (e1, e2, m, full (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    sparse_prod (const matrix_expression<E1> &e1,
                 const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        // FIXME needed for c_matrix?!
        // return sparse_prod (e1, e2, m, full (), false);
        return sparse_prod (e1, e2, m, full (), true);
    }

}}}

#endif


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::location_scale.hpp  //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_LOCATION_SCALE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_LOCATION_SCALE_HPP_ER_2009
#include <vector>
#include <string>
#include <algorithm>
#include <iterator>
#include <boost/mpl/assert.hpp>
#include <boost/utility.hpp>
#include <boost/type_traits.hpp> // remove_reference etc.
#include <boost/range.hpp>
#include <boost/statistics/detail/distribution_common/meta/inherit/policy.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_scalar_distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution_common/meta/policy.hpp>
#include <boost/math/policies/policy.hpp> // probably not needed
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    // Represents a location scale family
    //
    // X = mu + sigma * Z
    // pdf_X(x) = (1/sigma) pdf_Z((x-mu)/sigma)
    // cdf_X(x) = cdf_Z((x-mu)/sigma)
    //
    // TODO: range and support
    template<typename Z>
    class location_scale_distribution{
    	typedef std::string str_;
        static const str_ fun;

        public:

        typedef Z distribution_type;

        BOOST_MPL_ASSERT(( meta::is_scalar_distribution<Z> ));

        typedef typename distribution::meta::value<
            distribution_type
        >::type value_type;

        typedef typename distribution::meta::policy<
            distribution_type
        >::type policy_type;

        location_scale_distribution():mu_(0),sigma_(1),z_(){}
        location_scale_distribution(
            value_type mu,
            value_type sigma,
            const Z& z
        ):mu_(mu),sigma_(sigma),z_(z){
        	static const str_ str
            	= (fun + "<%1%>::location_scale_distribution").c_str();

            // TODO at present %1% is replaced by value_type. Write a function
            // that is implemented in terms of check_scale but actually puts
            // Z in its place

        	value_type result;
       	 	boost::math::detail::check_location(
            	str.c_str(), mu, &result, policy_type());
       	 	boost::math::detail::check_scale(
            	str.c_str(), sigma, &result, policy_type());
    	}

        location_scale_distribution(const location_scale_distribution& that)
        :mu_(that.mu_),sigma_(that.sigma_),z_(that.z_){}

        location_scale_distribution&
        operator=(const location_scale_distribution& that){
        	if(&that!=this){
            	mu_ = that.mu_;
            	sigma_ = that.sigma_;
            	z_ = that.z_;
       	 	}
        	return *this;
    	}

        value_type mu()const{ return this->mu_; }
        value_type sigma()const{ return this->sigma_; }
        const Z& z()const{ return this->z_; }

        private:
        value_type mu_;
        value_type sigma_;
        Z z_;
    };
    template<typename Z>
    const std::string location_scale_distribution<Z>::fun
    	= "distribution::toolkit::location_scale_distribution";

}// toolkit
}// distribution
}// detail
}// statistics
}// boost

#endif


/*
//@HEADER
// ************************************************************************
//
// qr_epetra_mv_householder_using_eigen_impl.hpp
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#ifndef QR_IMPL_EPETRA_QR_EPETRA_MV_HOUSEHOLDER_USING_EIGEN_IMPL_HPP_
#define QR_IMPL_EPETRA_QR_EPETRA_MV_HOUSEHOLDER_USING_EIGEN_IMPL_HPP_

#include <Eigen/OrderingMethods>
#include <Eigen/SparseQR>
#include <Epetra_Import.h>
#include <Epetra_Map.h>
#include <Epetra_LocalMap.h>

namespace pressio{ namespace qr{ namespace impl{

template<typename MatrixType, typename R_t>
class EpetraMVHouseholderUsingEigen
{

public:
  using sc_t = typename ::pressio::Traits<MatrixType>::scalar_type;
  using Q_type = Epetra_MultiVector;
  using eig_dyn_mat	= Eigen::MatrixXd;
  using help_impl_t	= QRHouseholderDenseEigenMatrix<eig_dyn_mat, R_t>;

private:
  help_impl_t myImpl_	= {};

public:
  EpetraMVHouseholderUsingEigen() = default;
  ~EpetraMVHouseholderUsingEigen() = default;

  template < typename VectorInType, typename VectorOutType>
  void applyQTranspose(const VectorInType & vecIn, VectorOutType & vecOut) const
  {
    constexpr auto beta  = ::pressio::utils::Constants<sc_t>::zero();
    constexpr auto alpha = ::pressio::utils::Constants<sc_t>::one();
    ::pressio::ops::product(::pressio::transpose(), alpha, *this->Qmat_, vecIn, beta, vecOut);
  }

  template <typename VectorType>
  void doLinSolve(const VectorType & rhs, VectorType & y)const
  {
    myImpl_.template doLinSolve<VectorType>(rhs, y);
  }

  const Q_type & QFactor() const
  {
    return *this->Qmat_;
  }

  template < typename VectorInType, typename VectorOutType>
  void applyRTranspose(const VectorInType & vecIn, VectorOutType & y) const
  {
    myImpl_.applyRTranspose(vecIn, y);
  }

  void computeThinOutOfPlace(const MatrixType & A)
  {
    auto rows = ::pressio::ops::extent(A,0);
    auto cols = ::pressio::ops::extent(A,1);
    auto & ArowMap = A.Map();

    // convert it to replicated eptra matrix
    Epetra_LocalMap locMap(rows, 0, A.Comm());
    Epetra_Import importer(locMap, ArowMap);
    MatrixType A2(locMap, cols);
    A2.Import(A, importer, Insert);

    // store it into an Eigen matrix
    eig_dyn_mat eA2W(rows,cols);
    for (int i=0;i<rows;i++){
      for (int j=0;j<cols;j++){
    	 eA2W(i,j) = A2[j][i];
      }
    }

    myImpl_.computeThinOutOfPlace(eA2W);

    // store Q into replicated Epetra_Multivector
    const auto & Q2 = myImpl_.QFactor();
    Q_type locQ(locMap,Q2.cols());
    for (int i=0;i<Q2.rows();i++){
      for (int j=0;j<Q2.cols();j++){
    	 locQ[j][i] = Q2(i,j);
      }
    }

    // import from local to distributed
    Qmat_ = std::make_shared<Q_type>(ArowMap, Q2.cols());
    Epetra_Import importer2(ArowMap, locMap);
    Qmat_->Import(locQ, importer2, Insert);
  }

private:
  mutable std::shared_ptr<Q_type> Qmat_	= nullptr;
  mutable std::shared_ptr<R_t> Rmat_	= nullptr;
};

}}} // end namespace pressio::qr::impl
#endif  // QR_IMPL_EPETRA_QR_EPETRA_MV_HOUSEHOLDER_USING_EIGEN_IMPL_HPP_


#pragma once

//          Copyright Rein Halbersma 2010-2021.
// Distributed under 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/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <cstddef>                                      // size_t

namespace dctl {

class Statistics
{
public:
        using NodeStats = boost::accumulators::accumulator_set<std::size_t, boost::accumulators::stats<boost::accumulators::tag::count> >;
        using PlyStats = boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::mean, boost::accumulators::tag::max> >;

        // modifiers

        void reset()
        {
                nodes_ = NodeStats();
                ply_ = PlyStats();
        }

        void collect(int ply)
        {
                nodes_(1);
                ply_(ply);
        }

        // observers

        NodeStats nodes() const
        {
                return nodes_;
        }

        PlyStats ply() const
        {
                return ply_;
        }

private:
        // representation

        NodeStats nodes_;
        PlyStats ply_;
};

}       // namespace dctl


/*
 * CSV_Reader.hpp
 *
 *  Created on: 16.01.2019
 *      Author: tomlucas
 */

#ifndef CSV_READER_HPP_
#define CSV_READER_HPP_

#include<string>
#include<vector>
#include <Eigen/Core>
#include<iostream>
#include<fstream>
#include <glog/logging.h>
namespace zavi
::csv_reader {

	/**
	 * Split a string at the delimiter char
	 * @param to_split the string to split
	 * @param delimiter the delimiter
	 * @return the splitted string as vector
	 */
	std::vector<std::string> splitString(const std::string & to_split, const char delimiter) ;

	/**
	 * parse a string to a double vector
	 * @param vector the string vector
	 * @return the parsed values
	 */
	std::vector<double> parseFloatVector(const std::vector<std::string> & vector);

	/**
	 * parse a string to an int vector
	 * @param vector the string vector
	 * @return the parsed values
	 */
	std::vector<int> parseIntVector(const std::vector<std::string> & vector);
	/**
	 * Parse an eigen matrix from a string vector
	 * @param vector the vector with all doubles
	 * @return the parsed values
	 */
	template<int size>
	Eigen::Matrix<double, size, 1> parseEigen(std::vector<std::string> & vector) {
		Eigen::Matrix<double, size, 1> floats;
		for (int i = 0; i < size; i++) {
			floats(i) = atof(vector[i].c_str());
		}
		return floats;
	}
	/**
	 * Reads a csv line to a double *
	 * @param csv the csv file ifstream
	 * @param result the pointer to put the result in (needs to be initialized)
	 * @param size  the line size/length
	 * @param delim the delimiter of the csv file
	 * @return whether the operation succeded
	 */
	bool readLineToPointer(std::ifstream &csv, double * result, const unsigned  int size,const char delim) ;
	/**
	 * Read a csv line to an eigen matrix
	 * @param csv the csv file stream
	 * @param result the matrix to store the line
	 * @param delim  the delimiter between values
	 * @return true if success
	 */
	template<int size>
	bool readLineToEigen(std::ifstream &csv, Eigen::Matrix<double,size,1> & result, const char delim=',') {
		std::string line;
		if(csv.good()) {
			getline(csv,line);

			auto string_list=splitString(line,delim);
			if(string_list.size() < size) {
				if (string_list.size() != 0)
				LOG(WARNING)<< "Line of file did not contain enough data. Expected: "<<size <<"Got: " << string_list.size();

				return false;
			}
			result=parseEigen<size>(string_list);
			return true;
		}
		else {
			LOG(WARNING)<< "CSV File is empty";
			return false;
		}
	}

}
//zavi::csv_reader

#endif /* CSV_READER_HPP_ */


// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_RANDOM_HH
#define DUNE_STUFF_RANDOM_HH

#include <random>
#include <limits>
#include <complex>

#include <boost/assign/list_of.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace Dune {
namespace Stuff {
namespace Common {

//! Helper class to abstract away selecting an integer or real valued distribution
template <typename T, bool = std::is_integral<T>::value>
struct UniformDistributionSelector
{
};

template <typename T>
struct UniformDistributionSelector<T, true>
{
  typedef std::uniform_int_distribution<T> type;
};
template <typename T>
struct UniformDistributionSelector<T, false>
{
  typedef std::uniform_real_distribution<T> type;
};

/** RandomNumberGenerator adapter
 * \template T type of generated numbers
 * \template EngineImp randomization algorithm/engine implementation
 * \template DistributionImp class with an EngineImp accepting call operator that returns the actual RNGs
 * \ref DefaultRNG for default choice of distribution, engine and init args
 **/
template <class T, class DistributionImp, class EngineImp = std::mt19937>
struct RNG
{
  typedef DistributionImp DistributionType;
  typedef EngineImp EngineType;
  EngineType generator;
  DistributionType distribution;
  RNG(EngineType g, DistributionType d) : generator(g), distribution(d) {}

  inline T operator()() { return distribution(generator); }
};

template <class T, class DistributionImp, class EngineImp>
struct RNG<std::complex<T>, DistributionImp, EngineImp>
{
  typedef DistributionImp DistributionType;
  typedef EngineImp EngineType;
  EngineType generator;
  DistributionType distribution;
  RNG(EngineType g, DistributionType d) : generator(g), distribution(d) {}

  inline std::complex<T> operator()() { return std::complex<T>(distribution(generator), distribution(generator)); }
};

namespace {
const std::string alphanums("abcdefghijklmnopqrstuvwxyz"
                            "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
                            "1234567890");
const std::string other_printables("!@#$%^&*()"
                                   "`~-_=+[{]{\\|;:'\",<.>/? ");
}

//! RNG that represents strings of given length
class RandomStrings : public RNG<std::string, std::uniform_int_distribution<int>, std::mt19937>
{
  typedef RNG<std::string, std::uniform_int_distribution<int>, std::mt19937> BaseType;
  const size_t length;

public:
  RandomStrings(size_t l)
    : BaseType(std::mt19937(std::random_device()()),
               std::uniform_int_distribution<int>(0, boost::numeric_cast<int>(alphanums.size() - 1)))
    , length(l)
  {
  }

  inline std::string operator()()
  {
    std::string ret(length, '\0');
    std::generate(std::begin(ret), std::end(ret), [=]() { return alphanums[distribution(generator)]; });
    return ret;
  }
};

//! defaultrng with choice of uniform distribution and stl's default random engine based on T and its numeric_limits
template <class T>
class DefaultRNG : public RNG<T, typename UniformDistributionSelector<T>::type, std::default_random_engine>
{
  typedef RNG<T, typename UniformDistributionSelector<T>::type, std::default_random_engine> BaseType;

public:
  DefaultRNG(T min = std::numeric_limits<T>::min(), T max = std::numeric_limits<T>::max(),
             T seed = std::random_device()())
    : BaseType(std::default_random_engine(seed), typename UniformDistributionSelector<T>::type(min, max))
  {
  }
};

template <class T>
class DefaultRNG<std::complex<T>>
    : public RNG<std::complex<T>, typename UniformDistributionSelector<T>::type, std::default_random_engine>
{
  typedef RNG<std::complex<T>, typename UniformDistributionSelector<T>::type, std::default_random_engine> BaseType;

public:
  DefaultRNG(T min = std::numeric_limits<T>::min(), T max = std::numeric_limits<T>::max(),
             T seed = std::random_device()())
    : BaseType(std::default_random_engine(seed), typename UniformDistributionSelector<T>::type(min, max))
  {
  }
};

template <>
class DefaultRNG<std::string> : public RandomStrings
{
public:
  DefaultRNG(size_t ilength = 12) : RandomStrings(ilength) {}
};

} // namespace Common
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_RANDOM_HH


/* -*- c++ -*- */
/*
 * Copyright 2015 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include <gnuradio/fec/tpc_decoder.h>
#include <gnuradio/fec/tpc_common.h>
#include <volk/volk.h>

#include <math.h>
#include <boost/assign/list_of.hpp>
#include <sstream>
#include <stdio.h>
#include <vector>

#include <algorithm>            // for std::reverse
#include <string.h>             // for memcpy

#include <gnuradio/fec/maxstar.h>

namespace gr {
 namespace fec {

generic_decoder::sptr
tpc_decoder::make(std::vector<int> row_polys, std::vector<int> col_polys, int krow, int kcol, int bval, int qval, int max_iter, int decoder_type)
{
    return generic_decoder::sptr(new tpc_decoder(row_polys, col_polys, krow, kcol, bval, qval, max_iter, decoder_type));
}

tpc_decoder::tpc_decoder (std::vector<int> row_polys, std::vector<int> col_polys, int krow, int kcol, int bval, int qval, int max_iter, int decoder_type)
    : generic_decoder("tpc_decoder"), d_rowpolys(row_polys), d_colpolys(col_polys), d_krow(krow), d_kcol(kcol),
      d_bval(bval), d_qval(qval), d_max_iter(max_iter), d_decoder_type(decoder_type)
{
    // first we operate on data chunks of get_input_size()
    // TODO: should we verify this and throw an error if it doesn't match?  YES
    // hwo do we do that?

    rowEncoder_K = ceil(log(d_rowpolys[0])/log(2));    // rowEncoder_K is the constraint length of the row encoder polynomial
    rowEncoder_n = d_rowpolys.size();
    rowEncoder_m = rowEncoder_K - 1;
    colEncoder_K = ceil(log(d_colpolys[0])/log(2));    // colEncoder_K is the constraint length of the col encoder polynomial
    colEncoder_n = d_colpolys.size();
    colEncoder_m = colEncoder_K - 1;

    // calculate the input and output sizes
    inputSize = ((d_krow+rowEncoder_m)*rowEncoder_n)*((d_kcol+colEncoder_m)*colEncoder_n) - d_bval;
    outputSize = (d_krow*d_kcol - (d_bval+d_qval));

    fp = NULL;
    //DEBUG_PRINT("inputSize=%d outputSize=%d\n", inputSize, outputSize);
    //fp = fopen("c_decoder_output.txt", "w");

    rowNumStates = 1 << (rowEncoder_m);       // 2^(row_mm)
    colNumStates = 1 << (colEncoder_m);       // 2^(col_mm)
    rowOutputs.resize(2, std::vector<int>(rowNumStates,0));
    rowNextStates.resize(2, std::vector<int>(rowNumStates,0));
    colOutputs.resize(2, std::vector<int>(colNumStates,0));
    colNextStates.resize(2, std::vector<int>(colNumStates,0));;

    // precalculate the state transition matrix for the row polynomial
    tpc_common::precomputeStateTransitionMatrix_RSCPoly(rowNumStates, d_rowpolys, rowEncoder_K, rowEncoder_n,
                                                        rowOutputs, rowNextStates);

    // precalculate the state transition matrix for the column polynomial
    tpc_common::precomputeStateTransitionMatrix_RSCPoly(colNumStates, d_colpolys, colEncoder_K, colEncoder_n,
                                                        colOutputs, colNextStates);

    codeword_M = d_kcol + colEncoder_K - 1;
    codeword_N = d_krow + rowEncoder_K - 1;

    // pre-allocate memory we use for encoding
    inputSizeWithPad = inputSize + d_bval;

    channel_llr.resize(codeword_M, std::vector<float>(codeword_N, 0));
    Z.resize(codeword_M, std::vector<float>(codeword_N, 0));
    extrinsic_info.resize(codeword_M*codeword_N, 0);

    input_u_rows.resize(d_krow, 0);
    input_u_cols.resize(d_kcol, 0);
    input_c_rows.resize(codeword_N, 0);
    input_c_cols.resize(codeword_M, 0);
    output_u_rows.resize(d_krow, 0);
    output_u_cols.resize(d_kcol, 0);
    output_c_rows.resize(codeword_N, 0);

    output_c_cols.resize(codeword_M*codeword_N, 0);
    output_c_col_idx = 0;

    // setup the max_star function based on decoder type
    switch(d_decoder_type) {
        case 0:
            max_star = &tpc_decoder::linear_log_map;
            break;
        case 1:
            max_star = &tpc_decoder::max_log_map;
            break;
        case 2:
            max_star = &tpc_decoder::constant_log_map;
            break;
        case 3:
            max_star = &tpc_decoder::log_map_lut_correction;
            break;
        case 4:
            max_star = &tpc_decoder::log_map_cfunction_correction;
            break;
        default:
            max_star = &tpc_decoder::linear_log_map;
            break;
    }

	// declare the reverse sweep trellis
	// the beta vector is logically laid out in memory as follows, assuming the
	// following values (for educational purposes)
	// defined: LL = 6, rowEncoder_K = 3, derived: mm_row=2, max_states_row=4, rowEncoder_n=1, num_symbols_row=2
	// state_idx
	//-------------------------------------------------------------------------
	//    0    B(0,0) <-- the calculated beta value at state=0, time=0
	//
	//    1    B(0,1)
	//
	//    2    B(0,2)
	//
	//    3    B(0,3)
	//-------------------------------------------------------------------------
	//k(time) =   0     1     2     3     4     5     6     7     8
    // setup row siso decoder
    mm_row = rowEncoder_K-1;                // encoder memory
    max_states_row = 1 << mm_row;       	// 2^mm_row
    num_symbols_row = 1 << rowEncoder_n;    // 2^rowEncoder_n

    beta_row.resize(input_u_rows.size()+rowEncoder_K, std::vector<float>(max_states_row, 0));
    // forward sweep trellis for current time instant only (t=k)
    alpha_prime_row.resize(max_states_row, 0);
    // forward sweep trellis for next time instant only    (t=k+1)
    alpha_row.resize(max_states_row, 0);
    // likelihood vector that input_c[idx] corresponds to a symbol in the set of [0 ... num_symbols_row-1]
    metric_c_row.resize(num_symbols_row, 0);
    // temporary vector to store the appropriate indexes of input_c that we want to test the likelihood of
    rec_array_row.resize(rowEncoder_n, 0);
    // log-likelihood ratios of the coded bit being a 1
    num_llr_c_row.resize(rowEncoder_n, 0);
    // log-likelihood ratios of the coded bit being a 0
    den_llr_c_row.resize(rowEncoder_n, 0);

    // setup column siso decoder
    mm_col = colEncoder_K-1;                // encoder memory
    max_states_col = 1 << mm_col;       	// 2^mm_col
    num_symbols_col = 1 << colEncoder_n;    // 2^colEncoder_n
    beta_col.resize(input_u_cols.size()+colEncoder_K, std::vector<float>(max_states_col, 0));
	// forward sweep trellis for current time instant only (t=k)
	alpha_prime_col.resize(max_states_col, 0);
	// forward sweep trellis for next time instant only    (t=k+1)
	alpha_col.resize(max_states_col, 0);
	// likelihood vector that input_c[idx] corresponds to a symbol in the set of [0 ... num_symbols_col-1]
	metric_c_col.resize(num_symbols_col, 0);
	// temporary vector to store the appropriate indexes of input_c that we want to test the likelihood of
	rec_array_col.resize(colEncoder_n, 0);
	// log-likelihood ratios of the coded bit being a 1
	num_llr_c_col.resize(colEncoder_n, 0);
	// log-likelihood ratios of the coded bit being a 0
	den_llr_c_col.resize(colEncoder_n, 0);

    mInit = (d_bval+d_qval)/d_krow;		// integer division
    nInit = (d_bval+d_qval) - mInit*d_krow;

    numInitLoadIter = d_bval/codeword_N;
	numInitRemaining = d_bval - codeword_N*numInitLoadIter;

}

int tpc_decoder::get_output_size() {
    return outputSize;
}

int tpc_decoder::get_input_size() {
    return inputSize;
}

// this code borrows heavily from CML library, please look @:
// http://code.google.com/p/iscml
// it has been refactored to work w/ gnuradio in C++ environment,
// as well as some comments being added to help understand
// exactly what is going on w/ the siso decoder
void tpc_decoder::siso_decode_row() {

    int LL, state, k, ii, symbol, mask;
    float app_in, delta1, delta2;
    LL = input_u_rows.size();        // code length

    // log-likelihood ratio of the uncoded bit being a 1
    float num_llr_u;
    // log-likelihood ratio of the uncoded bit being a 0
    float den_llr_u;

    // initialize beta_row trellis
    // this initialization is saying that the likelihood that the reverse sweep
    // starts at state=0 is 100%, b/c state 1, 2, 3's likelihood's are basically -inf
    // this implies that the forward trellis terminated at the 0 state
    //    for(state=1; state<max_states_row; state++) {
    //		beta_row[LL+rowEncoder_K-1][state] = -MAXLOG;
    //	}
    // filling w/ 0xCC yields a value close to -MAXLOG, and memset is faster than for loops
    memset(&beta_row[LL+rowEncoder_K-1][1], 0xCC, sizeof(float)*(max_states_row-1));

    // initialize alpha_prime_row (current time instant), alpha_prime_row then gets updated
    // by shifting in alpha_row at the end of each time instant of processing
    // alpha_row needs to get initialized at the beginning of each processing loop, so we
    // initialize alpha_row in the forward sweep processing loop
    // the initialization below is saying that the likelihood of starting at state=0 is
    // 100%, b/c state 1, 2, 3's likelihoods are basically -inf
    // as with the beta_row array, this implies that the forward trellis started at state=0
    //    for (state=1;state<max_states_row;state++) {
    //		alpha_prime_row[state] = -MAXLOG;
    //	}
    memset(&alpha_prime_row[1], 0xCC, sizeof(float)*(max_states_row-1));

    // compute the beta_row matrix first, which is the reverse sweep (hence we start at the last
    // time instant-1 and work our way back to t=0).  we start at last time instant-1 b/c
    // we already filled in beta_row values for the last time instant, forcing the trellis to
    // start at state=0
//     DEBUG_PRINT("LL=%d KK=%d mm=%d\n", LL, rowEncoder_K, mm_row);
//     DEBUG_PRINT_F(fp, "LL=%d KK=%d mm=%d\n", LL, rowEncoder_K, mm_row);
    for (k=(LL+(rowEncoder_K-1)-1); k>=0; k--) {
        // TODO: figure out why this is needed? I'm still confused by this
        if (k<LL) {
            app_in = input_u_rows[k];
        }
        else {
            app_in = 0;
        }

        // get the input associated w/ this time instant
        memcpy(&rec_array_row[0], &input_c_rows[rowEncoder_n*k], sizeof(float)*rowEncoder_n);

//         DEBUG_PRINT("k=%d\n", k);
//         DEBUG_PRINT_F(fp, "k=%d\n", k);
//
//         DEBUG_PRINT("rec_array -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&rec_array_row[0], rec_array_row.size());
//         DEBUG_PRINT_F(fp, "rec_array -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &rec_array_row[0], rec_array_row.size());

        // for each input at this time instant, create a metric which
        // represents the likelihood that this input corresponds to
        // each of the possible symbols
        for(symbol=0; symbol<num_symbols_row; symbol++) {
            metric_c_row[symbol] = gamma(rec_array_row, symbol);
        }

//         DEBUG_PRINT("metric_c -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&metric_c_row[0], metric_c_row.size());
//         DEBUG_PRINT_F(fp, "metric_c -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &metric_c_row[0], metric_c_row.size());

        // step through all states -- populating the beta_row values for each node
        // in the trellis diagram as shown above w/ the maximum-likelihood
        // of this current node coming from the previous node
        for ( state=0; state< max_states_row; state++ ) {
            // data 0 branch
            delta1 = beta_row[k+1][rowNextStates[0][state]] + metric_c_row[rowOutputs[0][state]];
            // data 1 branch
            delta2 = beta_row[k+1][rowNextStates[1][state]] + metric_c_row[rowOutputs[1][state]] + app_in;
            // update beta_row
            beta_row[k][state] = (*this.*max_star)(delta1, delta2);

//             DEBUG_PRINT("delta1=%f delta2=%f beta=%f\n", delta1, delta2, beta_row[k][state]);
//             DEBUG_PRINT_F(fp, "delta1=%f delta2=%f beta=%f\n", delta1, delta2, beta_row[k][state]);
        }

        // normalize beta_row
        for (state=1;state<max_states_row;state++) {
        	beta_row[k][state] = beta_row[k][state] - beta_row[k][0];
        }
        beta_row[k][0] = 0;

    }

    // compute the forward sweep (alpha_row values), and update the llr's
    // notice that we start at time index=1, b/c time index=0 has already been
    // initialized, and we are forcing the trellis to start from state=0
    for(k=1; k<LL+rowEncoder_K; k++) {
        // initialize the llr's for the uncoded bits
        num_llr_u = -MAXLOG;
        den_llr_u = -MAXLOG;

        // initialize alpha_row
//        for (state=0;state<max_states_row;state++) {
//			alpha_row[state] = -MAXLOG;
//		}
        memset(&alpha_row[0], 0xCC, max_states_row*sizeof(float));

        // assign inputs
        if (k-1 < LL)
            app_in = input_u_rows[k-1];
        else
            app_in = 0;

        // initialize the llr's for the coded bits, and get the correct
        // input bits for this time instant
        for (ii=0;ii<rowEncoder_n;ii++) {
            den_llr_c_row[ii] = -MAXLOG;
            num_llr_c_row[ii] = -MAXLOG;
            rec_array_row[ii] = input_c_rows[rowEncoder_n*(k-1)+ii];
        }

        // for each input at this time instant, create a metric which
        // represents the likelihood that this input corresponds to
        // each of the possible symbols
        for(symbol=0; symbol<num_symbols_row; symbol++) {
            metric_c_row[ii] = gamma(rec_array_row, symbol);
        }

        // compute the alpha_row vector
        // to understand the loop below, we need to think about the forward trellis.
        // we know that any node, at any time instant in the trellis diagram can have
        // multiple transitions into that node (i.e. any number of nodes in the
        // previous time instance can transition into this current node, based on the
        // polynomial).  SO, in the loop below, delta1 represents the transition from
        // the previous time instant for input (either 0 or 1) and the current state
        // we are looping over, and delta2 represents a transition from the previous
        // time instant to the current time instant for input (either 0 or 1) that
        // we've already calculated.  Essentially, b/c we can have multiple possible
        // transitions into the current alpha_row node of interest, and we need to store the
        // maximum likelihood of all those transitions, we keep delta2 as a previously
        // calculated transition, and then take the max* of that, which can be thought
        // of as a maximum (in the MAX-LOG-MAP approximation)
        for ( state=0; state<max_states_row; state++ ) {
            // Data 0 branch
            delta1 = alpha_prime_row[state] + metric_c_row[rowOutputs[0][state]];
            delta2 = alpha_row[rowNextStates[0][state]];
            alpha_row[rowNextStates[0][state]] = (*this.*max_star)(delta1, delta2);

            // Data 1 branch
            delta1 = alpha_prime_row[state] + metric_c_row[rowOutputs[1][state]] + app_in;
            delta2 = alpha_row[rowNextStates[1][state]];
            alpha_row[rowNextStates[1][state]] = (*this.*max_star)(delta1, delta2);
        }

        // compute the llr's
        for (state=0;state<max_states_row;state++)  {
            // data 0 branch (departing)
            delta1 = alpha_prime_row[state] + metric_c_row[rowOutputs[0][state]] + beta_row[k][rowNextStates[0][state]];
            // the information bit
            delta2 = den_llr_u;
            den_llr_u = (*this.*max_star)(delta1, delta2);
            mask = 1<<(rowEncoder_n-1);
            // go through all the code bits
            for (ii=0;ii<rowEncoder_n;ii++) {
                if ( rowOutputs[0][state]&mask ) {
                    // this code bit 1
                    delta2 = num_llr_c_row[ii];
                    num_llr_c_row[ii] = (*this.*max_star)(delta1, delta2);
                } else {
                    // this code bit is 0
                    delta2 = den_llr_c_row[ii];
                    den_llr_c_row[ii] = (*this.*max_star)(delta1, delta2);
                }
                mask = mask>>1;
            }

            // data 1 branch (departing)
            delta1 = alpha_prime_row[state] + metric_c_row[rowOutputs[1][state]] + beta_row[k][rowNextStates[1][state]] + app_in;
            // the information bit
            delta2 = num_llr_u;
            num_llr_u = (*this.*max_star)(delta1, delta2);
            mask = 1<<(rowEncoder_n-1);
            // go through all the code bits
            for (ii=0;ii<rowEncoder_n;ii++) {
                if ( rowOutputs[1][state]&mask ) {
                    // this code bit 1
                    delta2 = num_llr_c_row[ii];
                    num_llr_c_row[ii] = (*this.*max_star)(delta1, delta2);
                } else {
                    // this code bit is 0
                    delta2 = den_llr_c_row[ii];
                    den_llr_c_row[ii] = (*this.*max_star)(delta1, delta2);
                }
                mask = mask>>1;
            }

            // shift alpha_row back to alpha_prime_row
            alpha_prime_row[state] = alpha_row[state] - alpha_row[0];
        }

        // assign uncoded outputs
        if (k-1<LL) {
            output_u_rows[k-1] = num_llr_u - den_llr_u;
        }
        // assign coded outputs
        volk_32f_x2_subtract_32f(&output_c_rows[rowEncoder_n*(k-1)], &num_llr_c_row[0], &den_llr_c_row[0], rowEncoder_n);
    }
}

void tpc_decoder::siso_decode_col() {

    int LL, state, k, ii, symbol, mask;
    float app_in, delta1, delta2;
    LL = input_u_cols.size();        // code length

    // log-likelihood ratio of the uncoded bit being a 1
    float num_llr_u;
    // log-likelihood ratio of the uncoded bit being a 0
    float den_llr_u;

    // initialize beta_col trellis
    // this initialization is saying that the likelihood that the reverse sweep
    // starts at state=0 is 100%, b/c state 1, 2, 3's likelihood's are basically -inf
    // this implies that the forward trellis terminated at the 0 state
//    for(state=1; state<max_states_col; state++) {
//		beta_col[LL+colEncoder_K-1][state] = -MAXLOG;
//	}
    memset(&beta_col[LL+colEncoder_K-1][1], 0xCC, sizeof(float)*(max_states_col-1));

    // initialize alpha_prime_col (current time instant), alpha_prime_col then gets updated
    // by shifting in alpha_col at the end of each time instant of processing
    // alpha_col needs to get initialized at the beginning of each processing loop, so we
    // initialize alpha_col in the forward sweep processing loop
    // the initialization below is saying that the likelihood of starting at state=0 is
    // 100%, b/c state 1, 2, 3's likelihoods are basically -inf
    // as with the beta_col array, this implies that the forward trellis started at state=0
//    for (state=1;state<max_states_col;state++) {
//		alpha_prime_col[state] = -MAXLOG;
//	}
    memset(&alpha_prime_col[1], 0xCC, sizeof(float)*(max_states_col-1));

    // compute the beta_col matrix first, which is the reverse sweep (hence we start at the last
    // time instant-1 and work our way back to t=0).  we start at last time instant-1 b/c
    // we already filled in beta_col values for the last time instant, forcing the trellis to
    // start at state=0
//     DEBUG_PRINT("LL=%d KK=%d mm=%d\n", LL, colEncoder_K, mm_col);
//     DEBUG_PRINT_F(fp, "LL=%d KK=%d mm=%d\n", LL, colEncoder_K, mm_col);
    for (k=(LL+(colEncoder_K-1)-1); k>=0; k--) {
        // TODO: figure out why this is needed? I'm still confused by this
        if (k<LL) {
            app_in = input_u_cols[k];
        }
        else {
            app_in = 0;
        }

        // get the input associated w/ this time instant
        memcpy(&rec_array_col[0], &input_c_cols[colEncoder_n*k], sizeof(float)*colEncoder_n);

//         DEBUG_PRINT("k=%d\n", k);
//         DEBUG_PRINT_F(fp, "k=%d\n", k);
//
//         DEBUG_PRINT("rec_array -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&rec_array[0], rec_array_col.size());
//         DEBUG_PRINT_F(fp, "rec_array -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &rec_array[0], rec_array_col.size());

        // for each input at this time instant, create a metric which
        // represents the likelihood that this input corresponds to
        // each of the possible symbols
        for(symbol=0; symbol<num_symbols_col; symbol++) {
            metric_c_col[symbol] = gamma(rec_array_col, symbol);
        }

//         DEBUG_PRINT("metric_c -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&metric_c_col[0], metric_c_col.size());
//         DEBUG_PRINT_F(fp, "metric_c -->\n");
//         DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &metric_c_col[0], metric_c_col.size());

        // step through all states -- populating the beta_col values for each node
        // in the trellis diagram as shown above w/ the maximum-likelihood
        // of this current node coming from the previous node
        for ( state=0; state< max_states_col; state++ ) {
            // data 0 branch
        	delta1 = beta_col[k+1][colNextStates[0][state]] + metric_c_col[colOutputs[0][state]];
            // data 1 branch
            delta2 = beta_col[k+1][colNextStates[1][state]] + metric_c_col[colOutputs[1][state]] + app_in;
            // update beta_col
            beta_col[k][state] = (*this.*max_star)(delta1, delta2);

//             DEBUG_PRINT("delta1=%f delta2=%f beta=%f\n", delta1, delta2, beta_col[k][state]);
//             DEBUG_PRINT_F(fp, "delta1=%f delta2=%f beta=%f\n", delta1, delta2, beta_col[k][state]);
        }

        // normalize beta_col
        for (state=1;state<max_states_col;state++) {
        	beta_col[k][state] = beta_col[k][state] - beta_col[k][0];
        }
        beta_col[k][0] = 0;

    }

    // compute the forward sweep (alpha_col values), and update the llr's
    // notice that we start at time index=1, b/c time index=0 has already been
    // initialized, and we are forcing the trellis to start from state=0
    for(k=1; k<LL+colEncoder_K; k++) {
        // initialize the llr's for the uncoded bits
        num_llr_u = -MAXLOG;
        den_llr_u = -MAXLOG;

        // initialize alpha_col
//        for (state=0;state<max_states_col;state++) {
//			alpha_col[state] = -MAXLOG;
//		}
        memset(&alpha_col[0], 0xCC, max_states_col*sizeof(float));

        // assign inputs
        if (k-1 < LL)
            app_in = input_u_cols[k-1];
        else
            app_in = 0;

        // initialize the llr's for the coded bits, and get the correct
        // input bits for this time instant
        for (ii=0;ii<colEncoder_n;ii++) {
            den_llr_c_col[ii] = -MAXLOG;
            num_llr_c_col[ii] = -MAXLOG;
            rec_array_col[ii] = input_c_cols[colEncoder_n*(k-1)+ii];
        }

        // for each input at this time instant, create a metric which
        // represents the likelihood that this input corresponds to
        // each of the possible symbols
        for(symbol=0; symbol<num_symbols_col; symbol++) {
            metric_c_col[ii] = gamma(rec_array_col, symbol);
        }

        // compute the alpha_col vector
        // to understand the loop below, we need to think about the forward trellis.
        // we know that any node, at any time instant in the trellis diagram can have
        // multiple transitions into that node (i.e. any number of nodes in the
        // previous time instance can transition into this current node, based on the
        // polynomial).  SO, in the loop below, delta1 represents the transition from
        // the previous time instant for input (either 0 or 1) and the current state
        // we are looping over, and delta2 represents a transition from the previous
        // time instant to the current time instant for input (either 0 or 1) that
        // we've already calculated.  Essentially, b/c we can have multiple possible
        // transitions into the current alpha_col node of interest, and we need to store the
        // maximum likelihood of all those transitions, we keep delta2 as a previously
        // calculated transition, and then take the max* of that, which can be thought
        // of as a maximum (in the MAX-LOG-MAP approximation)
        for ( state=0; state<max_states_col; state++ ) {
            // Data 0 branch
            delta1 = alpha_prime_col[state] + metric_c_col[colOutputs[0][state]];
            delta2 = alpha_col[colNextStates[0][state]];
            alpha_col[colNextStates[0][state]] = (*this.*max_star)(delta1, delta2);

            // Data 1 branch
            delta1 = alpha_prime_col[state] + metric_c_col[colOutputs[1][state]] + app_in;
            delta2 = alpha_col[colNextStates[1][state]];
            alpha_col[colNextStates[1][state]] = (*this.*max_star)(delta1, delta2);
        }

        // compute the llr's
        for (state=0;state<max_states_col;state++)  {
            // data 0 branch (departing)
            delta1 = alpha_prime_col[state] + metric_c_col[colOutputs[0][state]] + beta_col[k][colNextStates[0][state]];
            // the information bit
            delta2 = den_llr_u;
            den_llr_u = (*this.*max_star)(delta1, delta2);
            mask = 1<<(colEncoder_n-1);
            // go through all the code bits
            for (ii=0;ii<colEncoder_n;ii++) {
                if ( colOutputs[0][state]&mask ) {
                    // this code bit 1
                    delta2 = num_llr_c_col[ii];
                    num_llr_c_col[ii] = (*this.*max_star)(delta1, delta2);
                } else {
                    // this code bit is 0
                    delta2 = den_llr_c_col[ii];
                    den_llr_c_col[ii] = (*this.*max_star)(delta1, delta2);
                }
                mask = mask>>1;
            }

            // data 1 branch (departing)
            delta1 = alpha_prime_col[state] + metric_c_col[colOutputs[1][state]] + beta_col[k][colNextStates[1][state]] + app_in;
            // the information bit
            delta2 = num_llr_u;
            num_llr_u = (*this.*max_star)(delta1, delta2);
            mask = 1<<(colEncoder_n-1);
            // go through all the code bits
            for (ii=0;ii<colEncoder_n;ii++) {
                if ( colOutputs[1][state]&mask ) {
                    // this code bit 1
                    delta2 = num_llr_c_col[ii];
                    num_llr_c_col[ii] = (*this.*max_star)(delta1, delta2);
                } else {
                    // this code bit is 0
                    delta2 = den_llr_c_col[ii];
                    den_llr_c_col[ii] = (*this.*max_star)(delta1, delta2);
                }
                mask = mask>>1;
            }

            // shift alpha_col back to alpha_prime_col
            alpha_prime_col[state] = alpha_col[state] - alpha_col[0];
        }

        // assign uncoded outputs
        if (k-1<LL) {
            output_u_cols[k-1] = num_llr_u - den_llr_u;
        }
        // assign coded outputs
        volk_32f_x2_subtract_32f(&output_c_cols[output_c_col_idx+rowEncoder_n*(k-1)], &num_llr_c_col[0], &den_llr_c_col[0], colEncoder_n);
    }
}

float tpc_decoder::linear_log_map(const float delta1, const float delta2) {
    float diff;

    diff = delta2 - delta1;

    if ( diff > TJIAN )
        return( delta2 );
    else if ( diff < -TJIAN )
        return( delta1 );
    else if ( diff > 0 )
        return( delta2 + AJIAN*(diff-TJIAN) );
    else
        return( delta1 - AJIAN*(diff+TJIAN) );
}

// MAX-LOG-MAP approximation
float tpc_decoder::max_log_map(const float delta1, const float delta2) {
    if(delta1>delta2) return delta1;
    return delta2;
}

float tpc_decoder::constant_log_map(const float delta1, const float delta2) {
    // Return maximum of delta1 and delta2
    // and in correction value if |delta1-delta2| < TVALUE
    register float diff;
    diff = delta2 - delta1;

    if ( diff > TVALUE )
        return( delta2 );
    else if ( diff < -TVALUE )
        return( delta1 );
    else if ( diff > 0 )
        return( delta2 + CVALUE );
    else
        return( delta1 + CVALUE );
}

float tpc_decoder::log_map_lut_correction(const float delta1, const float delta2) {
    float diff;
    diff = (float) fabs( delta2 - delta1 );

    if (delta1 > delta2) {
        if (diff > BOUNDARY8 )
            return( delta1 );
        else if ( diff > BOUNDARY4 ) {
            if (diff > BOUNDARY6 ) {
                if ( diff > BOUNDARY7 )
                    return( delta1 + VALUE7 + SLOPE7*(diff-BOUNDARY7) );
                else
                    return( delta1 + VALUE6 + SLOPE6*(diff-BOUNDARY6) );
            } else {
                if ( diff > BOUNDARY5 )
                    return( delta1 + VALUE5 + SLOPE5*(diff-BOUNDARY5) );
                else
                    return( delta1 + VALUE4 + SLOPE4*(diff-BOUNDARY4) );
            }
        } else {
            if (diff > BOUNDARY2 ) {
                if ( diff > BOUNDARY3 )
                    return( delta1 + VALUE3 + SLOPE3*(diff-BOUNDARY3) );
                else
                    return( delta1 + VALUE2 + SLOPE2*(diff-BOUNDARY2) );
            } else {
                if ( diff > BOUNDARY1 )
                    return( delta1 + VALUE1 + SLOPE1*(diff-BOUNDARY1) );
                else
                    return( delta1 + VALUE0 + SLOPE0*(diff-BOUNDARY0) );
            }
        }
    } else {
        if (diff > BOUNDARY8 )
            return( delta2 );
        else if ( diff > BOUNDARY4 ) {
            if (diff > BOUNDARY6 ) {
                if ( diff > BOUNDARY7 )
                    return( delta2 + VALUE7 + SLOPE7*(diff-BOUNDARY7) );
                else
                    return( delta2 + VALUE6 + SLOPE6*(diff-BOUNDARY6) );
            } else {
                if ( diff > BOUNDARY5 )
                    return( delta2 + VALUE5 + SLOPE5*(diff-BOUNDARY5) );
                else
                    return( delta2 + VALUE4 + SLOPE4*(diff-BOUNDARY4) );
            }
        } else {
            if (diff > BOUNDARY2 ) {
                if ( diff > BOUNDARY3 )
                    return( delta2 + VALUE3 + SLOPE3*(diff-BOUNDARY3) );
                else
                    return( delta2 + VALUE2 + SLOPE2*(diff-BOUNDARY2) );
            } else {
                if ( diff > BOUNDARY1 )
                    return( delta2 + VALUE1 + SLOPE1*(diff-BOUNDARY1) );
                else
                    return( delta2 + VALUE0 + SLOPE0*(diff-BOUNDARY0) );
            }
        }
    }
}

float tpc_decoder::log_map_cfunction_correction(const float delta1, const float delta2) {
    // Use C-function calls to compute the correction function
    if (delta1 > delta2) {
        return( (float) (delta1 + log( 1 + exp( delta2-delta1) ) ) );
    } else  {
        return( (float) (delta2 + log( 1 + exp( delta1-delta2) ) ) );
    }
}

float tpc_decoder::gamma(const std::vector<float> rx_array, const int symbol) {
    float rm = 0;
    int ii;
    int mask;
    int nn = rx_array.size();

    mask = 1;
    for (ii=0;ii<nn;ii++) {
        if (symbol&mask)
            rm += rx_array[nn-ii-1];
        mask = mask<<1;
    }

//     DEBUG_PRINT("nn=%d symbol=%d rm = %f\n", nn, symbol, rm);
//     DEBUG_PRINT_F(fp, "nn=%d symbol=%d rm = %f\n", nn, symbol, rm);

    return(rm);
}

template <typename T> int tpc_decoder::sgn(T val) {
    return (T(0) < val) - (val < T(0));
}

void tpc_decoder::generic_work(void *inBuffer, void *outBuffer) {
    const float *inPtr = (const float *) inBuffer;
    unsigned char *out = (unsigned char *) outBuffer;

    unsigned int m, n, ii;
    int iter;

    for(ii=0; ii<numInitLoadIter; ii++) {
    	memset(&channel_llr[ii][0], 0, sizeof(float)*codeword_N);
    	memset(&Z[ii][0], 0, sizeof(float)*codeword_N);
    }
    memset(&channel_llr[ii][0], 0, sizeof(float)*numInitRemaining);
    memset(&Z[ii][0], 0, sizeof(float)*numInitRemaining);
    memcpy(&channel_llr[ii][numInitRemaining], &inPtr[0], (codeword_N-numInitRemaining)*sizeof(float));
    memcpy(&Z[ii][numInitRemaining], &inPtr[0], (codeword_N-numInitRemaining)*sizeof(float));
    int inPtrIdx = codeword_N-numInitRemaining;
    for(ii=ii+1; ii<codeword_M; ii++) {
		memcpy(&channel_llr[ii][0], &inPtr[inPtrIdx], sizeof(float)*codeword_N);
		memcpy(&Z[ii][0], &inPtr[inPtrIdx], sizeof(float)*codeword_N);
		inPtrIdx += codeword_N;
	}

    // clear out extrinsic-info between blocks
    memset(&extrinsic_info[0], 0, sizeof(float)*extrinsic_info.size());

    //DEBUG_PRINT("Starting TURBO Decoding\n");

    for(iter=0; iter<d_max_iter; iter++) {
        //DEBUG_PRINT("Turbo Iter=%d\n", iter+1);
        //DEBUG_PRINT_F(fp, "Turbo Iter=%d\n", iter+1);
        // decode each row
        for(m=0; m<codeword_M; m++) {
            // stage the data
        	volk_32f_x2_add_32f(&input_c_rows[0], &channel_llr[m][0], &extrinsic_info[m*codeword_N], codeword_N);

            //DEBUG_PRINT("input_c_rows -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&input_c_rows[0], codeword_N);
            //DEBUG_PRINT_F(fp, "input_c_rows -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &input_c_rows[0], codeword_N);

            // call siso decode
            siso_decode_row();

            //DEBUG_PRINT("output_u_rows -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&output_u_rows[0], output_u_rows.size());
            //DEBUG_PRINT_F(fp, "output_u_rows -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &output_u_rows[0], output_u_rows.size());
            //DEBUG_PRINT("output_c_rows -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&output_c_rows[0], output_c_rows.size());
            //DEBUG_PRINT_F(fp, "output_c_rows -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &output_c_rows[0], output_c_rows.size());

            // copy the output coded back into Z, so we can feed it back through the decoder
            // for more iterations
            volk_32f_x2_subtract_32f(&Z[m][0], &output_c_rows[0], &extrinsic_info[m*codeword_N], codeword_N);
        }

        // decode each col
        earlyExit = true;
        output_c_col_idx = 0;
        for(n=0; n<codeword_N; n++) {
            // stage the data
            for(ii=0; ii<codeword_M; ii++) {
                input_c_cols[ii] = Z[ii][n];
            }

            //DEBUG_PRINT("input_c_cols -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&input_c_cols[0], codeword_M);
            //DEBUG_PRINT_F(fp, "input_c_cols -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &input_c_cols[0], codeword_M);

            // call siso decode
            siso_decode_col();

            //DEBUG_PRINT("output_u_cols -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&output_u_cols[0], output_u_cols.size());
            //DEBUG_PRINT_F(fp, "output_u_cols -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &output_u_cols[0], output_u_cols.size());
            //DEBUG_PRINT("output_c_cols -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT(&output_c_cols[output_c_col_idx], codeword_M);
            //DEBUG_PRINT_F(fp, "output_c_cols -->\n");
            //DEBUG_PRINT_FLOAT_ARRAY_AS_FLOAT_F(fp, &output_c_cols[output_c_col_idx], codeword_M);

            // create the extrinsic_info vector, which subtracts from input_c_cols to prevent feedback
            //DEBUG_PRINT("extrinsic_info -->\n");
            //DEBUG_PRINT_F(fp, "extrinsic_info -->\n");
            for(ii=0; ii<codeword_M; ii++) {
            	extrinsic_info[ii*codeword_N+n] = output_c_cols[output_c_col_idx+ii] - input_c_cols[ii];

            	if(earlyExit) {
            		if(sgn(output_c_cols[output_c_col_idx+ii])!=sgn(input_c_cols[ii])) {
						earlyExit = false;
					}
            	}

                //DEBUG_PRINT("%f ", extrinsic_info[ii*codeword_N+n]);
                //DEBUG_PRINT_F(fp, "%f ", extrinsic_info[ii*codeword_N+n]);
            }
            //DEBUG_PRINT("\n");
            //DEBUG_PRINT_F(fp, "\n");

            output_c_col_idx += codeword_M;
        }
        if(earlyExit) break;
    }

    ii=0;
	for(m=0; m<d_kcol; m++) {
		for(n=0; n<d_krow; n++) {
			if(ii==0) {
				m = mInit;
				n = nInit;
			}

			// make a hard decision
//			if(output_matrix[n*codeword_M+m]>0) {
			if(output_c_cols[n*codeword_M+m]>0) {
				out[ii++] = (unsigned char) 1;
			}
			else {
				out[ii++] = (unsigned char) 0;
			}
		}
	}

    //DEBUG_PRINT(("Output\n"));
    //DEBUG_PRINT_UCHAR_ARRAY(out, outputSize);
    //DEBUG_PRINT_F(fp, "Output\n");
    //DEBUG_PRINT_UCHAR_ARRAY_F(fp, out, outputSize);
}

int tpc_decoder::get_input_item_size() {
    return sizeof(INPUT_DATATYPE);
}

int tpc_decoder::get_output_item_size() {
    return sizeof(OUTPUT_DATATYPE);
}

int tpc_decoder::get_history() {
    return 0;
}

float tpc_decoder::get_shift() {
    return 0.0;
}

const char* tpc_decoder::get_conversion() {
    return "none";
}

tpc_decoder::~tpc_decoder()
{
  if(fp)
    fclose(fp);
}

}
}