// Filename: CirculationModel_RV_PA.cpp
// Created on 20 Aug 2007 by Boyce Griffith

// Modified 2019, Alexander D. Kaiser

#include "CirculationModel_RV_PA.h"
#include "pnpoly.h"
/////////////////////////////// INCLUDES /////////////////////////////////////

#ifndef included_IBAMR_config
#include <IBAMR_config.h>
#define included_IBAMR_config
#endif

#ifndef included_SAMRAI_config
#include <SAMRAI_config.h>
#define included_SAMRAI_config
#endif

// SAMRAI INCLUDES
#include <CartesianGridGeometry.h>
#include <CartesianPatchGeometry.h>
#include <PatchLevel.h>
#include <SideData.h>
#include <tbox/RestartManager.h>
#include <tbox/SAMRAI_MPI.h>
#include <tbox/Utilities.h>

// C++ STDLIB INCLUDES
#include <cassert>

#include <Eigen/Dense>
using namespace Eigen;

namespace
{
// Name of output file.
static const string DATA_FILE_NAME = "bc_data.m";

}

/////////////////////////////// NAMESPACE ////////////////////////////////////

/////////////////////////////// STATIC ///////////////////////////////////////

/////////////////////////////// PUBLIC ///////////////////////////////////////

CirculationModel_RV_PA::CirculationModel_RV_PA(Pointer<Database> input_db,
                                               const fourier_series_data *fourier_right_ventricle,
                                               const fourier_series_data *fourier_right_pa,
                                               const fourier_series_data *fourier_left_pa,
                                               string right_ventricle_vertices_file_name,
                                               string right_pa_vertices_file_name,
                                               string left_pa_vertices_file_name,
                                               const double  cycle_duration,
                                               const double  t_offset_bcs_unscaled,
                                               const double  initial_time,
                                               double P_initial_pa,
                                               bool rcr_bcs_on,
                                               bool resistance_bcs_on,
                                               bool inductor_bcs_on)
    :
      d_object_name("circ_model_rv_pa"),  // constant name here
      d_registered_for_restart(true),      // always true
      d_fourier_right_ventricle(fourier_right_ventricle),
      d_fourier_right_pa(fourier_right_pa),
      d_fourier_left_pa(fourier_left_pa),
      d_cycle_duration(cycle_duration),
      d_t_offset_bcs_unscaled(t_offset_bcs_unscaled),
      d_current_idx_series(0),
      d_Q_right_ventricle(0.0),
      d_Q_right_pa(0.0),
      d_Q_left_pa(0.0),
      d_Q_right_pa_previous(0.0),
      d_Q_left_pa_previous(0.0),
      d_time(initial_time),
      d_right_pa_P(P_initial_pa),
      d_right_pa_P_Wk(P_initial_pa),
      d_right_pa_P_distal(P_initial_pa),
      d_right_pa_P_distal_previous(P_initial_pa),
      d_left_pa_P(P_initial_pa),
      d_left_pa_P_Wk(P_initial_pa),
      d_left_pa_P_distal(P_initial_pa),
      d_left_pa_P_distal_previous(P_initial_pa),
      d_area_right_ventricle(0.0),
      d_area_right_pa(0.0),
      d_area_left_pa (0.0),
      d_area_initialized(false),
      d_rcr_bcs_on(rcr_bcs_on),
      d_resistance_bcs_on(resistance_bcs_on),
      d_inductor_bcs_on(inductor_bcs_on)
{

    if (d_registered_for_restart)
    {
        RestartManager::getManager()->registerRestartItem(d_object_name, this);
    }

    // Initialize object with data read from the input and restart databases.
    const bool from_restart = RestartManager::getManager()->isFromRestart();
    if (from_restart)
    {
        getFromRestart();
    }

    if (d_rcr_bcs_on){
        if (input_db){
            // left and right equal for now
            d_right_pa_R_proximal = input_db->getDouble("right_pa_R_proximal");
            d_right_pa_R_distal   = input_db->getDouble("right_pa_R_distal");
            d_right_pa_C          = input_db->getDouble("right_pa_C");

            d_left_pa_R_proximal  = input_db->getDouble("left_pa_R_proximal");
            d_left_pa_R_distal    = input_db->getDouble("left_pa_R_distal");
            d_left_pa_C           = input_db->getDouble("left_pa_C");

            std::cout << "input db got values:\n";
            std::cout << "right: R_proximal = " << d_right_pa_R_proximal << "\tR_distal = " << d_right_pa_R_distal << "\tC = " << d_right_pa_C << "\n";
            std::cout << "left : R_proximal = " << d_left_pa_R_proximal << "\tR_distal = " << d_left_pa_R_distal << "\tC = " << d_left_pa_C << "\n";
        }
        else {
            TBOX_ERROR("Must provide valid input_db");
        }
    }

    if (d_resistance_bcs_on){
        d_right_pa_resistance = input_db->getDouble("right_pa_R");
        d_left_pa_resistance  = input_db->getDouble("left_pa_R");
        std::cout << "input db got values:\n";
        std::cout << "right: resistance = " << d_right_pa_resistance << "\n";
        std::cout << "left : R_proximal = " << d_left_pa_resistance << "\n";
    }

    if (d_inductor_bcs_on){
        d_right_pa_inductance = input_db->getDouble("right_pa_L");
        d_left_pa_inductance  = input_db->getDouble("left_pa_L");
        std::cout << "input db got values:\n";
        std::cout << "right: inductance = " << d_right_pa_inductance << "\n";
        std::cout << "left : inductance = " << d_left_pa_inductance << "\n";
    }

    if ((d_rcr_bcs_on && d_resistance_bcs_on) ||
        (d_rcr_bcs_on && d_inductor_bcs_on)   ||
        (d_inductor_bcs_on && d_resistance_bcs_on)) {
        TBOX_ERROR("Cannot us two types of bc simulataneously");
    }

    double x,x_prev,y,y_prev,z,z_prev;
    double tol = 1.0e-2;

    // read vertices from file
    ifstream right_ventricle_file(right_ventricle_vertices_file_name.c_str(), ios::in);

    if(!right_ventricle_file){
        TBOX_ERROR("Aorta file not found\n");
    }

    right_ventricle_file >> d_n_pts_right_ventricle;

    d_right_ventricle_points_idx1 = new double[d_n_pts_right_ventricle];
    d_right_ventricle_points_idx2 = new double[d_n_pts_right_ventricle];

    for (int i=0; i<d_n_pts_right_ventricle; i++){
        right_ventricle_file >> x;
        right_ventricle_file >> d_right_ventricle_points_idx1[i];
        right_ventricle_file >> d_right_ventricle_points_idx2[i];

        if (i>0){
            if (fabs(x_prev - x) > tol){
                TBOX_ERROR("x coordinates must be consistent\n");
            }
        }
        x_prev = x;

    }
    pout << "to right_ventricle file close\n";
    right_ventricle_file.close();
    d_right_ventricle_axis = 0;
    d_right_ventricle_side = 0;

    // read vertices from file
    ifstream right_pa_file(right_pa_vertices_file_name.c_str(), ios::in);

    if(!right_pa_file){
        TBOX_ERROR("Aorta file not found\n");
    }

    right_pa_file >> d_n_pts_right_pa;

    d_right_pa_points_idx1 = new double[d_n_pts_right_pa];
    d_right_pa_points_idx2 = new double[d_n_pts_right_pa];

    for (int i=0; i<d_n_pts_right_pa; i++){
        right_pa_file >> d_right_pa_points_idx1[i];
        right_pa_file >> d_right_pa_points_idx2[i];
        right_pa_file >> z;

        if (i>0){
            if (fabs(z_prev - z) > tol){
                TBOX_ERROR("z coordinates must be consistent\n");
            }
        }
        z_prev = z;

    }
    pout << "to right_pa file close\n";
    right_pa_file.close();
    d_right_pa_axis = 2;
    d_right_pa_side = 0;

    // read vertices from file
    ifstream left_pa_file(left_pa_vertices_file_name.c_str(), ios::in);

    if(!left_pa_file){
        TBOX_ERROR("Left PA file not found\n");
    }

    left_pa_file >> d_n_pts_left_pa;

    d_left_pa_points_idx1 = new double[d_n_pts_left_pa];
    d_left_pa_points_idx2 = new double[d_n_pts_left_pa];

    for (int i=0; i<d_n_pts_left_pa; i++){
        left_pa_file >> d_left_pa_points_idx1[i];
        left_pa_file >> y;
        left_pa_file >> d_left_pa_points_idx2[i];

        if (i>0){
            if (fabs(y_prev - y) > tol){
                TBOX_ERROR("y coordinates must be consistent\n");
            }
        }
        y_prev = y;

    }
    pout << "to left_pa file close\n";
    left_pa_file.close();
    d_left_pa_axis = 1;
    d_left_pa_side = 0;

    pout << "passed contstructor\n";

    return;
} // CirculationModel

CirculationModel_RV_PA::~CirculationModel_RV_PA()
{
    return;
} // ~CirculationModel_RV_PA

void CirculationModel_RV_PA::advanceTimeDependentData(const double dt,
                                                        const Pointer<PatchHierarchy<NDIM> > hierarchy,
                                                        const int U_idx,
                                                        const int /*P_idx*/,
                                                        const int /*wgt_cc_idx*/,
                                                        const int wgt_sc_idx)
{
    // Compute the mean flow rates in the vicinity of the inflow and outflow
    // boundaries.

    double Q_right_ventricle_local = 0.0;
    double Q_right_pa_local = 0.0;
    double Q_left_pa_local = 0.0;

    double area_right_ventricle_local = 0.0;
    double area_right_pa_local = 0.0;
    double area_left_pa_local = 0.0;

    if (d_inductor_bcs_on){
        // save old values of Q for taking time derivatives
        d_Q_right_pa_previous = d_Q_right_pa;
        d_Q_left_pa_previous = d_Q_left_pa;
    }

    for (int ln = 0; ln <= hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = hierarchy->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            Pointer<CartesianPatchGeometry<NDIM> > pgeom = patch->getPatchGeometry();
            if (pgeom->getTouchesRegularBoundary())
            {
                Pointer<SideData<NDIM, double> > U_data = patch->getPatchData(U_idx);
                Pointer<SideData<NDIM, double> > wgt_sc_data = patch->getPatchData(wgt_sc_idx);
                const Box<NDIM>& patch_box = patch->getBox();
                const double* const x_lower = pgeom->getXLower();
                const double* const dx = pgeom->getDx();
                double dV = 1.0;
                for (int d = 0; d < NDIM; ++d)
                {
                    dV *= dx[d];
                }

                for(int axis=0; axis<3; axis++)
                {
                    for(int side=0; side<2; side++)
                    {
                        const bool is_lower = (side == 0);
                        if (pgeom->getTouchesRegularBoundary(axis, side))
                        {

                            Vector n;
                            for (int d = 0; d < NDIM; ++d)
                            {
                                n[d] = axis == d ? (is_lower ? -1.0 : +1.0) : 0.0;
                            }
                            Box<NDIM> side_box = patch_box;
                            if (is_lower)
                            {
                                side_box.lower(axis) = patch_box.lower(axis);
                                side_box.upper(axis) = patch_box.lower(axis);
                            }
                            else
                            {
                                side_box.lower(axis) = patch_box.upper(axis) + 1;
                                side_box.upper(axis) = patch_box.upper(axis) + 1;
                            }
                            for (Box<NDIM>::Iterator b(side_box); b; b++)
                            {
                                const Index<NDIM>& i = b();

                                double X[NDIM];
                                for (int d = 0; d < NDIM; ++d)
                                {
                                    X[d] = x_lower[d] + dx[d] * (double(i(d) - patch_box.lower(d)) + (d == axis ? 0.0 : 0.5));
                                }

                                double X_in_plane_1 = 0.0;
                                double X_in_plane_2 = 0.0;
                                if (axis == 0)
                                {
                                    X_in_plane_1 = X[1];
                                    X_in_plane_2 = X[2];
                                }
                                else if (axis == 1)
                                {
                                    X_in_plane_1 = X[0];
                                    X_in_plane_2 = X[2];
                                }
                                else if (axis == 2)
                                {
                                    X_in_plane_1 = X[0];
                                    X_in_plane_2 = X[1];
                                }
                                else{
                                    TBOX_ERROR("Invalid value of axis\n");
                                }

                                const int in_right_ventricle  = this->point_in_right_ventricle(X_in_plane_1, X_in_plane_2, axis, side);
                                const int in_right_pa         = this->point_in_right_pa       (X_in_plane_1, X_in_plane_2, axis, side);
                                const int in_left_pa          = this->point_in_left_pa        (X_in_plane_1, X_in_plane_2, axis, side);

                                if (in_right_ventricle && in_right_pa){
                                    TBOX_ERROR("Position is within two inlets and outlets, should be impossible\n");
                                }
                                if (in_right_ventricle && in_left_pa){
                                    TBOX_ERROR("Position is within two inlets and outlets, should be impossible\n");
                                }
                                if (in_right_pa && in_left_pa){
                                    TBOX_ERROR("Position is within two inlets and outlets, should be impossible\n");
                                }

                                if (in_right_ventricle)
                                {
                                    const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                                    if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                                    {
                                        double dA = dV / dx[axis];
                                        Q_right_ventricle_local += (*U_data)(i_s)* n[axis] * dA;

                                        if (!d_area_initialized){
                                            area_right_ventricle_local += dA;
                                        }

                                    }
                                }

                                if (in_right_pa)
                                {
                                    const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                                    if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                                    {
                                        double dA = dV / dx[axis];
                                        Q_right_pa_local += (*U_data)(i_s) * n[axis] * dA;

                                        if (!d_area_initialized){
                                            area_right_pa_local += dA;
                                        }

                                    }
                                }

                                if (in_left_pa)
                                {
                                    const SideIndex<NDIM> i_s(i, axis, SideIndex<NDIM>::Lower);
                                    if ((*wgt_sc_data)(i_s) > std::numeric_limits<double>::epsilon())
                                    {
                                        double dA = dV / dx[axis];
                                        Q_left_pa_local += (*U_data)(i_s) * n[axis] * dA;

                                        if (!d_area_initialized){
                                            area_left_pa_local += dA;
                                        }

                                    }
                                }

                            }
                        }
                    }
                }
            }
        }
    }

    d_Q_right_ventricle = SAMRAI_MPI::sumReduction(Q_right_ventricle_local);
    d_Q_right_pa        = SAMRAI_MPI::sumReduction(Q_right_pa_local);
    d_Q_left_pa         = SAMRAI_MPI::sumReduction(Q_left_pa_local);

    if (!d_area_initialized){
        d_area_right_ventricle = SAMRAI_MPI::sumReduction(area_right_ventricle_local);
        d_area_right_pa        = SAMRAI_MPI::sumReduction(area_right_pa_local);
        d_area_left_pa         = SAMRAI_MPI::sumReduction(area_left_pa_local);
        d_area_initialized = true;
    }

    // print_summary();

    // bool debug_out_areas = false;
    // if (debug_out_areas){
    //     pout << "d_area_right_ventricle = " << d_area_right_ventricle << "\n";
    //     pout << "d_area_right_pa = " << d_area_right_pa << "\n";
    //     pout << "d_area_left_pa = " << d_area_left_pa << "\n";
    // }

    d_time += dt;

    // compute which index in the Fourier series we need here
    // always use a time in current cycle
    double t_reduced = d_time - d_cycle_duration * floor(d_time/d_cycle_duration);

    // fourier series has its own period, scale to that
    double t_scaled = t_reduced * (d_fourier_right_ventricle->L  / d_cycle_duration);

    // start offset some arbitrary time in the cardiac cycle, but this is relative to the series length
    double t_scaled_offset = t_scaled + d_t_offset_bcs_unscaled;

    // Fourier data here
    // index without periodicity
    unsigned int k = (unsigned int) floor(t_scaled_offset / (d_fourier_right_ventricle->dt));

    // // take periodic reduction
    d_current_idx_series = k % (d_fourier_right_ventricle->N_times);

    if (d_rcr_bcs_on){
        // The downstream pressure is determined by a three-element Windkessel model.

        double coeff_left = (d_left_pa_C / dt + 1.0 / d_left_pa_R_distal);
        double coeff_right = (d_right_pa_C / dt + 1.0 / d_right_pa_R_distal);

        // grab the downstream pressures
        d_right_pa_P_distal_previous = d_right_pa_P_distal;
        d_right_pa_P_distal = MMHG_TO_CGS * d_fourier_right_pa->values[d_current_idx_series];

        d_left_pa_P_distal_previous = d_left_pa_P_distal;
        d_left_pa_P_distal = MMHG_TO_CGS * d_fourier_left_pa->values[d_current_idx_series];

        // hooked to ground version
        // d_right_pa_P_Wk = ((d_right_pa_C / dt) * d_right_pa_P_Wk + d_Q_right_pa) / (d_right_pa_C / dt + 1.0 / d_right_pa_R_distal);
        // d_right_pa_P = d_right_pa_P_Wk + d_right_pa_R_proximal * d_Q_right_pa;

        // d_left_pa_P_Wk = ((d_left_pa_C / dt) * d_left_pa_P_Wk + d_Q_left_pa) / (d_left_pa_C / dt + 1.0 / d_left_pa_R_distal);
        // d_left_pa_P = d_left_pa_P_Wk + d_left_pa_R_proximal * d_Q_left_pa;

        d_right_pa_P_Wk = ((d_right_pa_C / dt) * (d_right_pa_P_Wk - d_right_pa_P_distal_previous) + coeff_right*d_right_pa_P_distal + d_Q_right_pa) / coeff_right;
        d_right_pa_P = d_right_pa_P_Wk + d_right_pa_R_proximal * d_Q_right_pa;

        d_left_pa_P_Wk = ((d_left_pa_C / dt) * (d_left_pa_P_Wk - d_left_pa_P_distal_previous) + coeff_right*d_left_pa_P_distal + d_Q_left_pa) / coeff_left;
        d_left_pa_P = d_left_pa_P_Wk + d_left_pa_R_proximal * d_Q_left_pa;

    }
    else if (d_resistance_bcs_on){
        // pressure upstream of resistance determined by series
        d_right_pa_P_Wk = MMHG_TO_CGS * d_fourier_right_pa->values[d_current_idx_series];
        d_left_pa_P_Wk  = MMHG_TO_CGS * d_fourier_left_pa->values[d_current_idx_series];

        // resistance bcs determine outlet pressure
        d_right_pa_P = d_right_pa_P_Wk + d_right_pa_resistance * d_Q_right_pa;
        d_left_pa_P  = d_left_pa_P_Wk  + d_left_pa_resistance  * d_Q_left_pa;
    }

    else if (d_inductor_bcs_on){
        // pressure upstream of resistance determined by series
        d_right_pa_P_Wk = MMHG_TO_CGS * d_fourier_right_pa->values[d_current_idx_series];
        d_left_pa_P_Wk  = MMHG_TO_CGS * d_fourier_left_pa->values[d_current_idx_series];

        // inductance bcs determine update on pressure
        d_right_pa_P = d_right_pa_P_Wk + d_right_pa_inductance * (d_Q_right_pa - d_Q_right_pa_previous)/dt;
        d_left_pa_P  = d_left_pa_P_Wk  + d_left_pa_inductance  * (d_Q_left_pa  - d_Q_left_pa_previous )/dt ;
    }

    else {
        d_right_pa_P = MMHG_TO_CGS * d_fourier_right_pa->values[d_current_idx_series];
        d_left_pa_P  = MMHG_TO_CGS * d_fourier_left_pa->values[d_current_idx_series];
    }

    // bool debug_out = false;
    // if (debug_out){
    //     pout << "circ mode: d_time = " << d_time << ", d_current_idx_series = " << d_current_idx_series << "\n";
    //     pout << "t_reduced = " << t_reduced << " t_scaled = " << t_scaled << " t_scaled_offset = " << t_scaled_offset << "\n";
    //     pout << "k (unreduced idx) = " << k << " d_current_idx_series = " << d_current_idx_series << "\n\n";
    // }

    writeDataFile();

} // advanceTimeDependentData

void CirculationModel_RV_PA::set_Q_valve(double Q_valve){
    d_Q_valve = Q_valve;
}

void
CirculationModel_RV_PA::putToDatabase(Pointer<Database> db)
{

    db->putInteger("d_current_idx_series", d_current_idx_series);
    db->putDouble("d_Q_right_ventricle", d_Q_right_ventricle);
    db->putDouble("d_Q_right_pa", d_Q_right_pa);
    db->putDouble("d_Q_left_pa", d_Q_left_pa);
    db->putDouble("d_Q_right_pa_previous", d_Q_right_pa_previous);
    db->putDouble("d_Q_left_pa_previous", d_Q_left_pa_previous);
    db->putDouble("d_Q_valve", d_Q_valve);
    db->putDouble("d_right_pa_P", d_right_pa_P);
    db->putDouble("d_right_pa_P_Wk", d_right_pa_P_Wk);
    db->putDouble("d_right_pa_P_distal", d_right_pa_P_distal);
    db->putDouble("d_right_pa_P_distal_previous", d_right_pa_P_distal_previous);
    db->putDouble("d_left_pa_P",d_left_pa_P);
    db->putDouble("d_left_pa_P_Wk",d_left_pa_P_Wk);
    db->putDouble("d_left_pa_P_distal", d_left_pa_P_distal);
    db->putDouble("d_left_pa_P_distal_previous", d_left_pa_P_distal_previous);
    db->putDouble("d_time", d_time);
    db->putBool("d_rcr_bcs_on", d_rcr_bcs_on);
    db->putBool("d_resistance_bcs_on", d_resistance_bcs_on);
    db->putBool("d_inductor_bcs_on", d_inductor_bcs_on);
    return;
} // putToDatabase

void CirculationModel_RV_PA::print_summary(){

    double P_right_ventricle = d_fourier_right_ventricle->values[d_current_idx_series];
    double P_right_pa;
    double P_left_pa;

    if (d_rcr_bcs_on){
        P_right_pa        = d_right_pa_P / MMHG_TO_CGS;
        P_left_pa         = d_left_pa_P / MMHG_TO_CGS;
    }
    else{
        P_right_pa        = d_fourier_right_pa->values[d_current_idx_series];
        P_left_pa         = d_fourier_left_pa->values[d_current_idx_series];
    }

    pout << "rcr_bcs_on = " << d_rcr_bcs_on << "\n";
    pout << "% time \t P_right_ventricle (mmHg)\t P_right_pa (mmHg)\t P_left_pa (mmHg)\t Q_right_ventricle (ml/s)\t d_Q_right_pa (ml/s)\t d_Q_right_pa (ml/s)\tQ_valve (ml/s) \t idx" ;
    if (d_rcr_bcs_on || d_resistance_bcs_on){
        pout << "\t right_pa_P_Wk\t left_pa_P_Wk\t ";
    }
    pout << "\n";
    pout << d_time << " " << P_right_ventricle <<  " " << P_right_pa << " " << P_left_pa << " " << d_Q_right_ventricle << " " << d_Q_right_pa << " " << d_Q_left_pa << " " << d_Q_valve << " " << d_current_idx_series;
    if (d_rcr_bcs_on || d_resistance_bcs_on){
        pout  << " " << d_right_pa_P_Wk << " " << d_left_pa_P_Wk;
    }
    pout << "\n";

}

int CirculationModel_RV_PA::point_in_right_ventricle(double testx, double testy, int axis, int side){
    // checks whether given point is in right ventricle

    // quick exit for correct side and axis
    if ((axis != d_right_ventricle_axis) || (side != d_right_ventricle_side))
        return 0;

    return pnpoly(d_n_pts_right_ventricle, d_right_ventricle_points_idx1, d_right_ventricle_points_idx2, testx, testy);
}

int CirculationModel_RV_PA::point_in_right_pa(double testx, double testy, int axis, int side){
    // checks whether given point is in right ventricle

    // quick exit for correct side and axis
    if ((axis != d_right_pa_axis) || (side != d_right_pa_side))
        return 0;

    return pnpoly(d_n_pts_right_pa, d_right_pa_points_idx1, d_right_pa_points_idx2, testx, testy);
}

int CirculationModel_RV_PA::point_in_left_pa(double testx, double testy, int axis, int side){
    // checks whether given point is in right ventricle

    // quick exit for correct side and axis
    if ((axis != d_left_pa_axis) || (side != d_left_pa_side))
        return 0;

    return pnpoly(d_n_pts_left_pa, d_left_pa_points_idx1, d_left_pa_points_idx2, testx, testy);
}

void CirculationModel_RV_PA::write_plot_code()
{
    static const int mpi_root = 0;
    if (SAMRAI_MPI::getRank() == mpi_root)
    {
        ofstream fout(DATA_FILE_NAME.c_str(), ios::app);
        fout.setf(ios_base::scientific);
        fout.setf(ios_base::showpos);
        fout.precision(10);

        fout << "];\n";
        fout << "MMHG_TO_CGS = 1333.22368;\n";
        fout << "fig = figure;\n";
        fout << "times   =  bc_vals(:,1);\n";
        fout << "p_rv    =  bc_vals(:,2);\n";
        fout << "p_rpa   =  bc_vals(:,3);\n";
        fout << "p_lpa   =  bc_vals(:,4);\n";
        fout << "q_rv    = -bc_vals(:,5); \n";
        fout << "q_rpa   =  bc_vals(:,6);\n";
        fout << "q_lpa   =  bc_vals(:,7);\n";
        fout << "q_valve =  bc_vals(:,8);\n";
        fout << "p_wk_rpa =  bc_vals(:,9);\n";
        fout << "p_wk_lpa =  bc_vals(:,10);\n";
        fout << "load '../bc_variables_experimental.mat'\n";
        fout << "subplot(2,1,1)\n";
        fout << "plot(times, p_rv, 'k')\n";
        fout << "hold on\n";
        fout << "plot(times, p_rpa, ':k')\n";
        fout << "plot(times, p_lpa, '-.k')\n";
        fout << "plot(times_exp, p_rv_exp)\n";
        fout << "plot(times_exp, p_pa_exp)\n";
        fout << "plot(times, p_wk_rpa)\n";
        fout << "plot(times, p_wk_lpa)\n";
        fout << "% legend('P RV', 'P RPA', 'PWK RPA', 'P LPA', 'PWK LPA', 'P EXP RV', 'P EXP PA', Location','NorthEastOutside');\n";
        fout << "legend('P RV', 'P RPA', 'P LPA', 'P EXP RV', 'P EXP PA', 'WK RPA', 'WK LPA', 'Location','NorthEastOutside');\n";
        fout << "xlabel('t (s)')\n";
        fout << "ylabel('P (mmHg)')\n";
        fout << "subplot(2,1,2)\n";
        fout << "plot(times, q_rv, 'k')\n";
        fout << "hold on\n";
        fout << "plot(times, q_rpa, '--k')\n";
        fout << "plot(times, q_lpa, '-.k')\n";
        fout << "plot(times_two_cycles, q_rv_exp)\n";
        fout << "plot(bc_vals(:,1), zeros(size(q_rv)), ':k')\n";
        fout << "legend('Q RV', 'Q RPA', 'Q LPA', 'Q EXP RV', 'Location', 'NorthEastOutside')\n";
        fout << "xlabel('t (s)')\n";
        fout << "ylabel('Flow (ml/s)')\n";
        fout << "set(fig, 'Position', [100, 100, 1000, 750])\n";
        fout << "set(fig,'PaperPositionMode','auto')\n";
        fout << "printfig(fig, 'bc_model_variables_with_experimental')\n";
        fout << "q_mean = mean(q_rv)\n";
    }
    return;
}

/////////////////////////////// PROTECTED ////////////////////////////////////

/////////////////////////////// PRIVATE //////////////////////////////////////

void
    CirculationModel_RV_PA::writeDataFile() const
{
    static const int mpi_root = 0;
    if (SAMRAI_MPI::getRank() == mpi_root)
    {
        static bool file_initialized = false;
        const bool from_restart = RestartManager::getManager()->isFromRestart();
        if (!from_restart && !file_initialized)
        {
            ofstream fout(DATA_FILE_NAME.c_str(), ios::out);
            fout << "% time \t P_right_ventricle (mmHg)\t P_right_pa (mmHg)\t P_left_pa (mmHg)\t d_Q_right_ventricle (ml/s)\t d_Q_right_pa (ml/s)\td_Q_left_pa (ml/s) \td_Q_valve (ml/s) \t d_right_pa_P_Wk \t d_left_pa_P_Wk";
            if (d_rcr_bcs_on){
                fout << "d_right_pa_P_distal \t d_left_pa_P_distal";
            }
            fout << "\n"
                 << "bc_vals = [";
            file_initialized = true;
        }

        ofstream fout(DATA_FILE_NAME.c_str(), ios::app);

        fout << d_time;
        fout.setf(ios_base::scientific);
        fout.setf(ios_base::showpos);
        fout.precision(10);

        double P_right_ventricle = d_fourier_right_ventricle->values[d_current_idx_series];

        fout << " " << P_right_ventricle <<  " " << d_right_pa_P/MMHG_TO_CGS << " " << d_left_pa_P/MMHG_TO_CGS;
        fout << " " << d_Q_right_ventricle << " " << d_Q_right_pa << " " << d_Q_left_pa << " " << d_Q_valve;
        fout << " " << d_right_pa_P_Wk/MMHG_TO_CGS << " " << d_left_pa_P_Wk/MMHG_TO_CGS;
        if(d_rcr_bcs_on){
            fout << " " << d_right_pa_P_distal/MMHG_TO_CGS << " " << d_left_pa_P_distal/MMHG_TO_CGS;
        }
        fout << "; \n";

    }

    return;
} // writeDataFile

void
CirculationModel_RV_PA::getFromRestart()
{
    Pointer<Database> restart_db = RestartManager::getManager()->getRootDatabase();
    Pointer<Database> db;
    if (restart_db->isDatabase(d_object_name))
    {
        db = restart_db->getDatabase(d_object_name);
    }
    else
    {
        TBOX_ERROR("Restart database corresponding to " << d_object_name << " not found in restart file.");
    }

    d_current_idx_series         = db->getInteger("d_current_idx_series");
    d_Q_right_ventricle          = db->getDouble("d_Q_right_ventricle");
    d_Q_right_pa                 = db->getDouble("d_Q_right_pa");
    d_Q_left_pa                  = db->getDouble("d_Q_left_pa");
    d_Q_right_pa_previous        = db->getDouble("d_Q_right_pa_previous");
    d_Q_left_pa_previous         = db->getDouble("d_Q_left_pa_previous");
    d_Q_valve                    = db->getDouble("d_Q_valve");
    d_right_pa_P                 = db->getDouble("d_right_pa_P");
    d_right_pa_P_Wk              = db->getDouble("d_right_pa_P_Wk");
    d_right_pa_P_distal          = db->getDouble("d_right_pa_P_distal");
    d_right_pa_P_distal_previous = db->getDouble("d_right_pa_P_distal_previous");
    d_left_pa_P                  = db->getDouble("d_left_pa_P");
    d_left_pa_P_Wk               = db->getDouble("d_left_pa_P_Wk");
    d_left_pa_P_distal           = db->getDouble("d_left_pa_P_distal");
    d_left_pa_P_distal_previous  = db->getDouble("d_left_pa_P_distal_previous");
    d_time                       = db->getDouble("d_time");
    d_rcr_bcs_on                 = db->getBool("d_rcr_bcs_on");
    d_resistance_bcs_on          = db->getBool("d_resistance_bcs_on");
    d_inductor_bcs_on            = db->getBool("d_inductor_bcs_on");
    return;
} // getFromRestart

/////////////////////////////// NAMESPACE ////////////////////////////////////

/////////////////////////////// TEMPLATE INSTANTIATION ///////////////////////

//////////////////////////////////////////////////////////////////////////////


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ScreenedRutherfordElasticElectronScatteringDistribution.hpp
//! \author Luke Kersting
//! \brief  The screened Rutherford elastic electronscattering distribution base class
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_SCREENED_RUTHERFORD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_SCREENED_RUTHERFORD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "MonteCarlo_ParticleBank.hpp"
#include "Utility_TabularDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"
#include "MonteCarlo_ElectronScatteringDistribution.hpp"
#include "MonteCarlo_AdjointElectronScatteringDistribution.hpp"
#include "MonteCarlo_AnalogElasticElectronScatteringDistribution.hpp"

namespace MonteCarlo{

//! The scattering distribution base class
class ScreenedRutherfordElasticElectronScatteringDistribution : public ElectronScatteringDistribution,
                                    public AdjointElectronScatteringDistribution
{

public:

  //! Typedef for the array of energy dependent screened rutherford paramters
  //! (first = energy, second = Moliere screening constant,
  //!  third = normalization constant
  typedef Teuchos::Array<Utility::Trip<double,double,double> > ParameterArray;

  typedef Teuchos::RCP<const AnalogElasticElectronScatteringDistribution>
            ElasticDistribution;

  //! Constructor from ACE table data
  ScreenedRutherfordElasticElectronScatteringDistribution(
    const ElasticDistribution& elastic_cutoff_distribution,
    const int atomic_number,
    const double upper_cutoff_angle = 1.0e-6 );

  //! Constructor from ENDL table data
  ScreenedRutherfordElasticElectronScatteringDistribution(
    const ParameterArray& screened_rutherford_parameters,
    const double upper_cutoff_angle = 1.0e-6 );

  //! Destructor
  virtual ~ScreenedRutherfordElasticElectronScatteringDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  double evaluate( const double incoming_energy,
                   const double scattering_angle ) const;

  //! Evaluate the PDF
  double evaluatePDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Evaluate the integrated PDF
  double evaluateIntegratedPDF( const double incoming_energy) const;

  //! Evaluate the CDF
  double evaluateCDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Sample an outgoing energy and direction from the distribution
  void sample( const double incoming_energy,
               double& outgoing_energy,
               double& scattering_angle ) const;

  //! Sample an outgoing energy and direction and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
                              double& outgoing_energy,
                              double& scattering_angle,
                              unsigned& trials ) const;

  //! Randomly scatter the electron
  void scatterElectron( ElectronState& electron,
                        ParticleBank& bank,
                        SubshellType& shell_of_interaction ) const;

  //! Randomly scatter the adjoint electron
  void scatterAdjointElectron( AdjointElectronState& adjoint_electron,
                               ParticleBank& bank,
                               SubshellType& shell_of_interaction ) const;

//protected:

   //! Sample an outgoing direction from the distribution
  void sampleAndRecordTrialsImpl( const double incoming_energy,
                                  double& scattering_angle_cosine,
                                  unsigned& trials ) const;

  //! Evaluate Moliere's atomic screening constant at the given electron energy
  double evaluateMoliereScreeningConstant( const double energy ) const;

private:

  // Find the lower and upper bin boundary
  void findLowerAndUpperBinBoundary(
        const double incoming_energy,
        ParameterArray::const_iterator& lower_bin_boundary,
        ParameterArray::const_iterator& upper_bin_boundary,
        double& interpolation_fraction ) const;

  // evaluate the pdf integrated from 0 to angle
  double evaluateIntegratedPDF(
        const double& scattering_angle,
        const ParameterArray::const_iterator& lower_bin_boundary,
        const ParameterArray::const_iterator& upper_bin_boundary,
        const double& interpolation_fraction ) const;

  // The fine structure constant (fsc) squared
  static double s_fine_structure_const_squared;

  // A parameter for moliere's screening factor  (1/2*(fsc/0.885)**2)
  static double s_screening_param1;

  // Atomic number (Z) of the target atom
  int d_atomic_number;

  // Atomic number (Z) of the target atom to the 2/3 power (Z^2/3)
  double d_Z_two_thirds_power;

  // A parameter for moliere's screening factor (3.76*fsc**2*Z**2)
  double d_screening_param2;

  // The scattering angle below which the screened Rutherford distribution is used
  double d_upper_cutoff_angle;

  // The scattering angle cosine above which the screened Rutherford distribution is used
  double d_lower_cutoff_angle_cosine;

  // Flag to indicate that tabulated screened rutherford parameters are used
  bool d_using_endl_tables;

  // Analog elastic scattering distribution
  ElasticDistribution d_elastic_cutoff_distribution;

  // Screened Rutherford energy depended paramters: Moliere's screening constant and normalization constant
  ParameterArray d_screened_rutherford_parameters;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_SCREENED_RUTHERFORD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ScreenedRutherfordElasticElectronScatteringDistribution.hpp
//---------------------------------------------------------------------------//


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoCuNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "CBSubsetOperator.h"
#include "osMOMSolverD.h"
#include "osMOMSolverD3.h"
#include "cuSolverUtils.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <solvers/osMOMSolverD3.h>
#include "hoLinearResampleOperator_eigen.h"
#include "multiplicationOperatorContainer.h"
#include "cuLinearResampleOperator.h"
#include "multiresRegistrationSolver.h"
#include "cuCGHSOFSolver.h"

#include "hdf5_utils.h"
#include "hoCuOFPartialDerivativeOperator.h"
#include "hoCuTVOFPartialDerivativeOperator.h"

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr< hoCuNDArray<float> >
perform_registration( boost::shared_ptr< hoCuNDArray<float> > volume, float of_alpha, float of_beta, unsigned int num_multires_levels )
{
	std::vector<size_t> volume_dims_3d = *volume->get_dimensions();
	volume_dims_3d.pop_back();
	std::vector<size_t> volume_dims_3d_3 = volume_dims_3d;
	volume_dims_3d_3.push_back(3);

	size_t num_elements_3d = volume_dims_3d[0]* volume_dims_3d[1]* volume_dims_3d[2];
	size_t num_phases = volume->get_size(3);

	boost::shared_ptr< hoCuNDArray<float> > host_result_field( new hoCuNDArray<float> );
	{
		std::vector<size_t> volume_dims_4d = volume_dims_3d;
		volume_dims_4d.push_back(3);
		volume_dims_4d.push_back(volume->get_size(3));
		host_result_field->create( &volume_dims_4d );
	}

	// Upload host data to device
	//

	for( unsigned int i=0; i<num_phases; i++ ){

		hoCuNDArray<float> host_fixed( &volume_dims_3d, volume->get_data_ptr()+((i+1)%num_phases)*num_elements_3d );
		hoCuNDArray<float> host_moving( &volume_dims_3d, volume->get_data_ptr()+i*num_elements_3d );

		cuNDArray<float> fixed_image(&host_fixed);
		cuNDArray<float> moving_image(&host_moving);

		boost::shared_ptr< cuLinearResampleOperator<float,3> > R( new cuLinearResampleOperator<float,3>() );

		// Setup solver
		//
		cuCGHSOFSolver<float,3> OFs;
		//cuCKOpticalFlowSolver<float,3> OFs;
		OFs.set_interpolator( R );
		OFs.set_output_mode( cuCGHSOFSolver<float,3>::OUTPUT_VERBOSE );
		OFs.set_max_num_iterations_per_level( 500 );
		OFs.set_num_multires_levels( num_multires_levels );
		OFs.set_alpha(of_alpha);
		//OFs.set_beta(of_beta);
		//OFs.set_limit(0.01f);

		// Run registration
		//
		boost::shared_ptr< cuNDArray<float> > result = OFs.solve( &fixed_image, &moving_image );
        std::cout << "Dimensions of vector field ";
        auto dims = *result->get_dimensions();
        for (size_t d : dims) std::cout << d << " ";
        std::cout << std::endl;

		cuNDArray<float> dev_sub;
		dev_sub.create( &volume_dims_3d_3, result->get_data_ptr() );

		hoCuNDArray<float> host_sub( &volume_dims_3d_3, host_result_field->get_data_ptr()+i*num_elements_3d*3 );
		host_sub = dev_sub;

	}

	/*
 {
  std::cout << std::endl << "Writing out registration results for phase " << phase << "." << std::endl;
  char filename[256];
  sprintf(&(filename[0]), "def_moving_%i.real", phase);
  write_nd_array<float>(&host_result_image, (char*)filename);
 }
	 */

	return host_result_field;
}
class hoCuConvertOperator : public subsetOperator<hoCuNDArray<float>>{

public:
	hoCuConvertOperator(boost::shared_ptr<subsetOperator<hoCuNDArray<float>>> _op) : subsetOperator<hoCuNDArray<float>>(_op->get_number_of_subsets()),op(_op){};

	virtual void mult_M(hoCuNDArray<float> *in, hoCuNDArray<float> *out,int subset, bool accumulate) override { op->mult_M((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset,accumulate);}
	virtual void mult_MH(hoCuNDArray<float> *in, hoCuNDArray<float> *out,int subset, bool accumulate) override { op->mult_MH((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset, accumulate);}
	virtual void mult_MH_M(hoCuNDArray<float> *in, hoCuNDArray<float> *out,int subset, bool accumulate) override { op->mult_MH_M((hoCuNDArray<float>*)in,(hoCuNDArray<float>*) out, subset, accumulate);}

	virtual boost::shared_ptr< std::vector<size_t> > get_codomain_dimensions(int i) { return op->get_codomain_dimensions(i);}
	virtual boost::shared_ptr< std::vector<size_t> > get_domain_dimensions() { return op->get_domain_dimensions();}
	virtual void set_domain_dimensions( std::vector<size_t> * dims) { return op->set_domain_dimensions(dims);}
	virtual void set_codomain_dimensions( std::vector<size_t> * dims) { return op->set_codomain_dimensions(dims);}

protected:
	boost::shared_ptr<subsetOperator<hoCuNDArray<float>>> op;
};

boost::shared_ptr<hoCuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
	std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics=binning->get_3d_binning();
	std::vector<size_t> is_dims3d = is_dims;
	is_dims3d.pop_back();
	boost::shared_ptr< hoCuConebeamProjectionOperator >
	Ep( new hoCuConebeamProjectionOperator() );
	Ep->setup(ps,binning_pics,imageDimensions);
	Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	Ep->set_domain_dimensions(&is_dims3d);
	Ep->set_use_filtered_backprojection(true);
	boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	Ep->mult_MH(&projections,prior3d.get());

	hoCuNDArray<float> tmp_proj(*ps->get_projections());
	Ep->mult_M(prior3d.get(),&tmp_proj);
	float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	*prior3d *= s;
	boost::shared_ptr<hoCuNDArray<float> > prior(new hoCuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	std::cout << "Prior complete" << std::endl;
	return prior;
}

int main(int argc, char** argv)
{
	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int iterations;
	floatd2 scale_factor;
	unsigned int subsets;
	int reg_iter;
	float rho;
	float tv_weight;
    float tv_weight4d;
    float tau;
	po::options_description desc("Allowed options");
	string tv_prior_filename;
	desc.add_options()
    		("help", "produce help message")
    		("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    		("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    		("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    		("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    		("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    		("SAG","Use exact SAG correction if present")
    		("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    		("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    		("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    		("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
					("downsample,D",po::value<floatd2>(&scale_factor)->default_value(floatd2(1,1)),"Downsample projections this factor")
    		("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    		("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight")
            ("TV4D",po::value<float>(&tv_weight4d)->default_value(0),"Total variation in temporal direction")
                    ("tau",po::value<float>(&tau)->default_value(1e-5),"Solver tau")
					("reg_iter",po::value<int>(&reg_iter)->default_value(2),"Regularization iterations")
    		("use_prior","Use an FDK prior")
    		("TV-prior",po::value<string>(&tv_prior_filename)->default_value("reconstructionTV.real"),"TV prior for registration")
                    ("vector-field",po::value<string>(),"Stored vector field")
    		("3D","Only use binning data to determine wrong projections")
    		;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);
    if (scale_factor[0] != 1 || scale_factor[1] != 1)
        ps->downsample(scale_factor[0],scale_factor[1]);

	float SDD = ps->get_geometry()->get_SDD();
	float SAD = ps->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = ps->get_projections()->get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	// Define encoding matrix
	auto E = boost::make_shared<CBSubsetOperator<hoCuNDArray> >(subsets);

	//E->setup(ps,binning,imageDimensions);
	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

	auto E2 = boost::make_shared<hoCuConvertOperator>(E);

	//hoCuGPBBSolver<float> solver;
	//hoCuCgDescentSolver<float> solver;
//	osSPSSolver<hoCuNDArray<float>> solver;
	osMOMSolverD<hoCuNDArray<float>> solver;
	//osSPSSolver<hoCuNDArray<float>> solver;
	//hoCuNCGSolver<float> solver;
	solver.set_encoding_operator(E);
	//solver.set_domain_dimensions(&is_dims);
	solver.set_max_iterations(iterations);
	solver.set_output_mode(hoCuGPBBSolver<float>::OUTPUT_VERBOSE);
	solver.set_non_negativity_constraint(true);
    solver.set_tau(tau);
	solver.set_reg_steps(reg_iter);
	//solver.set_rho(rho);

	hoCuNDArray<float> projections = *ps->get_projections();
	E->offset_correct(&projections);

	boost::shared_ptr<hoCuNDArray<float> > prior;

	if (vm.count("use_prior")) {
		prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
		solver.set_x0(prior);
	}
	/*
	if (tv_weight > 0){
		auto total_variation = boost::make_shared<hoCuTvOperator<float,4>>();
		total_variation->set_weight(tv_weight);
		//total_variation->set_weight_array(weight_array);
		solver.add_nonlinear_operator(total_variation);
		solver.set_kappa(tv_weight);
}
*/

    if (tv_weight4d > 0){
        boost::shared_ptr<hoNDArray<float>> displacements;
        if (vm.count("vector-field")){
            displacements = read_nd_array<float>(vm["vector-field"].as<string>().c_str());
            auto vdims = *displacements->get_dimensions();
            for (auto v : vdims) std::cout << v << " ";
            std::cout<<std::endl;
        } else {
            auto tv_recon = boost::make_shared<hoCuNDArray<float>>();
            *tv_recon = *read_nd_array<float>(tv_prior_filename.c_str());
            displacements = perform_registration(tv_recon, 0.01, 1, 3);
        }
        //clear(displacements.get());
        //auto DtOF = boost::make_shared<hoCuOFPartialDerivativeOperator<float>>();

		auto DtOF = boost::make_shared<hoCuTVOFPartialDerivativeOperator<float>>();
		//auto DtOF = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(3);
        DtOF->set_weight(tv_weight4d);
        DtOF->set_displacement_field(displacements);
        DtOF->set_domain_dimensions(&is_dims);
		auto is_dimsTV = is_dims;
		is_dimsTV.push_back(3);
        DtOF->set_codomain_dimensions(&is_dimsTV);
        solver.add_regularization_operator(DtOF);
    }

  if (tv_weight > 0){

  	auto Dx = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(0);
  	Dx->set_weight(tv_weight);
  	Dx->set_domain_dimensions(&is_dims);
  	Dx->set_codomain_dimensions(&is_dims);

  	auto Dy = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(1);
  	Dy->set_weight(tv_weight);
  	Dy->set_domain_dimensions(&is_dims);
  	Dy->set_codomain_dimensions(&is_dims);

  	auto Dz = boost::make_shared<hoCuPartialDerivativeOperator<float,4>>(2);
  	Dz->set_weight(tv_weight);
  	Dz->set_domain_dimensions(&is_dims);
  	Dz->set_codomain_dimensions(&is_dims);
	solver.add_regularization_group({Dx,Dy,Dz});
  }

	auto result = solver.solve(&projections);

	//write_nd_array<float>( result.get(), outputFile.c_str());
	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),"",iterations);
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_MaxwellFissionDistribution_def.hpp
//! \author Aaron Tumulak
//! \brief  Maxwell Fission distribution class definition. Modified by Alex
//!         Robinson to accommodate units.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_MAXWELL_FISSION_DISTRIBUTION_DEF_HPP
#define UTILITY_MAXWELL_FISSION_DISTRIBUTION_DEF_HPP

// Boost Includes
#include <boost/units/cmath.hpp>

// FRENSIE Includes
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_ArrayString.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_ExceptionTestMacros.hpp"
#include "Utility_ExceptionCatchMacros.hpp"
#include "Utility_ContractException.hpp"

namespace Utility{

// Constructor
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution(
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity incident_energy,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity nuclear_temperature,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity restriction_energy,
  const double constant_multiplier )
  : d_incident_energy( incident_energy ),
    d_nuclear_temperature( nuclear_temperature ),
    d_restriction_energy( restriction_energy ),
    d_multiplier( DMQT::initializeQuantity( constant_multiplier ) ),
    d_norm_constant()
{
  // Make sure values are valid
  testPrecondition( !IQT::isnaninf( incident_energy ) );
  testPrecondition( !IQT::isnaninf( nuclear_temperature ) );
  testPrecondition( !IQT::isnaninf( restriction_energy ) );
  testPrecondition( !QT::isnaninf( constant_multiplier ) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( incident_energy > IQT::zero() );
  testPrecondition( nuclear_temperature > IQT::zero() );
  // Make sure that the constant multiplier is positive
  testPrecondition( constant_multiplier > 0.0 );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Constructor
/*! \details This constructor will explicitly cast the input quantities to
 * the distribution quantity (which includes any unit-conversion). The
 * dimension type must match and there must be a unit-conversion defined using
 * the boost methodology.
 */
template<typename IndependentUnit, typename DependentUnit>
template<typename InputIndepQuantityA,
	 typename InputIndepQuantityB,
	 typename InputIndepQuantityC>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution(
				const InputIndepQuantityA incident_energy,
				const InputIndepQuantityB nuclear_temperature,
				const InputIndepQuantityC restriction_energy,
				const double constant_multiplier )
  : d_incident_energy( incident_energy ),
    d_nuclear_temperature( nuclear_temperature ),
    d_restriction_energy( restriction_energy ),
    d_multiplier( DMQT::initializeQuantity( constant_multiplier ) ),
    d_norm_constant()
{
  // Make sure values are valid
  testPrecondition( !QuantityTraits<InputIndepQuantityA>::isnaninf( incident_energy ) );
  testPrecondition( !QuantityTraits<InputIndepQuantityB>::isnaninf( nuclear_temperature ) );
  testPrecondition( !QuantityTraits<InputIndepQuantityC>::isnaninf( restriction_energy ) );
  testPrecondition( !QT::isnaninf( constant_multiplier ) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( incident_energy >
		    QuantityTraits<InputIndepQuantityA>::zero() );
  testPrecondition( nuclear_temperature >
		    QuantityTraits<InputIndepQuantityB>::zero() );
  // Make sure that the constant multiplier is positive
  testPrecondition( constant_multiplier > 0.0 );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Copy constructor
/*! \details Just like boost::units::quantity objects, the unit-aware
 * distribution can be explicitly cast to a distribution with compatible
 * units. If the units are not compatible, this function will not compile. Note
 * that this allows distributions to be scaled safely (unit conversions
 * are completely taken care of by boost::units)!
 */
template<typename IndependentUnit, typename DependentUnit>
template<typename InputIndepUnit, typename InputDepUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution(
  const UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>& dist_instance )
  : d_incident_energy( dist_instance.d_incident_energy ),
    d_nuclear_temperature( dist_instance.d_nuclear_temperature ),
    d_restriction_energy( dist_instance.d_restriction_energy ),
    d_multiplier(),
    d_norm_constant()
{
  // Make sure the multipliers are valid
  remember( typedef QuantityTraits<typename UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>::IndepQuantity> InputIQT );
  testPrecondition( !InputIQT::isnaninf( dist_instance.d_incident_energy ) );
  testPrecondition( !InputIQT::isnaninf( dist_instance.d_nuclear_temperature));
  testPrecondition( !InputIQT::isnaninf( dist_instance.d_restriction_energy) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( dist_instance.d_incident_energy > InputIQT::zero() );
  testPrecondition( dist_instance.d_nuclear_temperature > InputIQT::zero() );

  // Calculate the scaled multiplier (for complex units, boost::units often has
  // problems doing the conversion so we will do it manually)
  d_multiplier = getRawQuantity( dist_instance.d_multiplier )*DepQuantity( QuantityTraits<typename UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>::DepQuantity>::one() )/Utility::sqrt( IndepQuantity( QuantityTraits<typename UnitAwareMaxwellFissionDistribution<InputIndepUnit,InputDepUnit>::IndepQuantity>::one() ) );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Copy constructor
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::UnitAwareMaxwellFissionDistribution( const UnitAwareMaxwellFissionDistribution<void,void>& unitless_dist_instance, int )
  : d_incident_energy( IQT::initializeQuantity( unitless_dist_instance.d_incident_energy ) ),
    d_nuclear_temperature( IQT::initializeQuantity( unitless_dist_instance.d_nuclear_temperature ) ),
    d_restriction_energy( IQT::initializeQuantity( unitless_dist_instance.d_restriction_energy ) ),
    d_multiplier( DMQT::initializeQuantity( unitless_dist_instance.d_multiplier ) ),
    d_norm_constant()
{
  // Make sure the multipliers are valid
  testPrecondition( !QT::isnaninf( unitless_dist_instance.d_incident_energy ) );
  testPrecondition( !QT::isnaninf( unitless_dist_instance.d_nuclear_temperature) );
  testPrecondition( !QT::isnaninf( unitless_dist_instance.d_restriction_energy) );
  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( unitless_dist_instance.d_incident_energy > 0.0 );
  testPrecondition( unitless_dist_instance.d_nuclear_temperature > 0.0 );

  // Calculate the norm constant
  this->calculateNormalizationConstant();
}

// Construct distribution from a unitless dist. (potentially dangerous)
/*! \details Constructing a unit-aware distribution from a unitless
 * distribution is potentially dangerous. By forcing users to construct objects
 * using this method instead of a standard constructor we are trying to make
 * sure users are aware of the danger. This is designed to mimic the interface
 * of the boost::units::quantity, which also has to deal with this issue.
 */
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::fromUnitlessDistribution( const UnitAwareMaxwellFissionDistribution<void,void>& unitless_distribution )
{
  return ThisType( unitless_distribution, 0 );
}

// Assignment operator
template<typename IndependentUnit, typename DependentUnit>
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>&
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::operator=(
    const UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>& dist_instance )
{
  // Make sure the distribution is valid
  testPrecondition( !IQT::isnaninf( dist_instance.d_incident_energy ) );
  testPrecondition( !IQT::isnaninf( dist_instance.d_nuclear_temperature ) );
  testPrecondition( !IQT::isnaninf( dist_instance.d_restriction_energy ) );
  testPrecondition( !DMQT::isnaninf( dist_instance.d_multiplier ) );
  testPrecondition( dist_instance.d_incident_energy > IQT::zero() );
  testPrecondition( dist_instance.d_nuclear_temperature > IQT::zero() );

  if( this != &dist_instance )
  {
    d_incident_energy = dist_instance.d_incident_energy;
    d_nuclear_temperature = dist_instance.d_nuclear_temperature;
    d_restriction_energy = dist_instance.d_restriction_energy;
    d_multiplier = dist_instance.d_multiplier;
    d_norm_constant = dist_instance.d_norm_constant;
  }

  return *this;
}

// Evaluate the distribution
/*! \details This is simply the unnormalized distribution. An implicit
 * conversion factor (1.0) is used to convert to the correct units.
 */
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::DepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::evaluate(
 const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity indep_var_value ) const
{
  if( indep_var_value < IQT::zero() )
    return DQT::zero();
  else
  {
    return d_multiplier*Utility::sqrt( indep_var_value )*
      exp( -indep_var_value / d_nuclear_temperature );
  }
}

// Evaluate the PDF
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::InverseIndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::evaluatePDF(
const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity indep_var_value ) const
{
  return this->evaluate( indep_var_value )*d_norm_constant;
}

// Return a random sample from the distribution
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sample() const
{
  return ThisType::sample( d_incident_energy,
			   d_nuclear_temperature,
			   d_restriction_energy );
}

// Return a random sample from the corresponding CDF and record the number of trials
template<typename IndependentUnit, typename DependentUnit>
inline typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sample(
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity incident_energy,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity nuclear_temperature,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity restriction_energy )
{
  unsigned trials = 0;

  return ThisType::sampleAndRecordTrials( incident_energy,
					  nuclear_temperature,
					  restriction_energy,
					  trials );
}

// Return a random sample and record the number of trials
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sampleAndRecordTrials( unsigned& trials ) const
{
  return ThisType::sampleAndRecordTrials( d_incident_energy,
					  d_nuclear_temperature,
					  d_restriction_energy,
					  trials );
}

// Return a random sample from the corresponding CDF and record the number of trials
template<typename IndependentUnit, typename DependentUnit>
inline typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::sampleAndRecordTrials(
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity incident_energy,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity nuclear_temperature,
  const typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity restriction_energy,
  unsigned& trials )
{
  // Make sure values are valid
  testPrecondition( !IQT::isnaninf( incident_energy ) );
  testPrecondition( !IQT::isnaninf( nuclear_temperature ) );
  testPrecondition( !IQT::isnaninf( restriction_energy ) );

  // Make sure that incident energy and nuclear temperature is positive
  testPrecondition( incident_energy > IQT::zero() );
  testPrecondition( nuclear_temperature > IQT::zero() );

  double random_number_1, random_number_2, random_number_3;
  double term_1, term_2, arg;
  IndepQuantity sample;

  // Use the method outlined in OpenMC documentation under "5.7.2.4. ACE Law 7 - UnitAwareMaxwell Fission Spectrum"
  while( true )
  {
    // Increment the trial counter
    ++trials;

    random_number_1 = RandomNumberGenerator::getRandomNumber<double>();
    random_number_2 = RandomNumberGenerator::getRandomNumber<double>();
    random_number_3 = RandomNumberGenerator::getRandomNumber<double>();

    term_1 = log(random_number_1);

    arg = cos( PhysicalConstants::pi * random_number_3 * 0.5 );

    term_2 = log(random_number_2)*arg*arg;

    sample = -nuclear_temperature * ( term_1 + term_2 );

    if( sample <= (incident_energy - restriction_energy) )
      break;
  }

  return sample;
}

// Calculate the normalization constant of the distribution
/*
 * As given by ENDF Law 7
 * c^(-1) = T^(3/2)*[(sqrt(pi)/2)*erf(sqrt((E-U)/T)) - sqrt((E-U)/T)*exp(-(E-U)/T)]
 */
template<typename IndependentUnit, typename DependentUnit>
void
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::calculateNormalizationConstant()
{
  double argument = (d_incident_energy - d_restriction_energy) / d_nuclear_temperature;

  d_norm_constant =  rpow<3,-2>(d_nuclear_temperature)/
    (d_multiplier*(sqrt( PhysicalConstants::pi ) * 0.5 * erf( sqrt(argument) )-
		   sqrt(argument) * exp(-argument) ) );
}

// Return the upper bound of the distribution independent variable
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::getUpperBoundOfIndepVar() const
{
  return (d_incident_energy - d_restriction_energy);
}

// Return the lower bound of the distribution independent variable
template<typename IndependentUnit, typename DependentUnit>
typename UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::IndepQuantity
UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::getLowerBoundOfIndepVar() const
{
  return IQT::zero();
}

// Return the distribution type
template<typename IndependentUnit, typename DependentUnit>
OneDDistributionType UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::getDistributionType() const
{
  return ThisType::distribution_type;
}

// Test if the distribution is continuous
template<typename IndependentUnit, typename DependentUnit>
bool UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::isContinuous() const
{
  return true;
}

// Method for placing the object in an output stream
template<typename IndependentUnit, typename DependentUnit>
void UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::toStream( std::ostream& os ) const
{
  os << "{" << getRawQuantity( d_incident_energy )
     << "," << getRawQuantity( d_nuclear_temperature )
     << "," << getRawQuantity( d_restriction_energy );

  // Only print the multiplier when a scaling has been done
  if( d_multiplier != DMQT::one() )
    os << "," << getRawQuantity( d_multiplier ) << "}";
  else
    os << "}";
}

// Method for initializing the object from an input stream
template<typename IndependentUnit, typename DependentUnit>
void UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::fromStream( std::istream& is )
{
  // Read in the distribution representation
  std::string dist_rep;
  std::getline( is, dist_rep, '}' );
  dist_rep += '}';

  Teuchos::Array<std::string> distribution;
  try{
    distribution = Teuchos::fromStringToArray<std::string>( dist_rep );
  }
  catch( Teuchos::InvalidArrayStringRepresentation& error )
  {
    std::string message( "Error: the Maxwell Fission distribution cannot be "
                        "constructed because the representation is not valid "
                        "(see details below)!\n" );
    message += error.what();

    throw InvalidDistributionStringRepresentation( message );
  }

  TEST_FOR_EXCEPTION( distribution.size() > 4,
                     InvalidDistributionStringRepresentation,
                     "Error: the Maxwell Fission distribution cannot "
                     "be constructed because the representation is "
                     "not valid"
                     "(only 4 values or fewer  may be specified)!" );

  // Set the incient neutron energy
  if( distribution.size() > 0 )
  {
    TEST_FOR_EXCEPTION( distribution[0].find_first_not_of( " 0123456789.e" ) <
			distribution[0].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid incident energy "
			<< distribution[0] );
    {
      double incident_energy;

      std::istringstream iss( distribution[0] );
      Teuchos::extractDataFromISS( iss, incident_energy );

      setQuantity( d_incident_energy, incident_energy );
    }

    TEST_FOR_EXCEPTION( IQT::isnaninf( d_incident_energy ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid incident energy "
			<< d_incident_energy );

    TEST_FOR_EXCEPTION( d_incident_energy < IQT::zero(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid incident energy "
			<< d_incident_energy );
  }

  // Set the nuclear temperature
  if( distribution.size() > 1 )
  {
    TEST_FOR_EXCEPTION( distribution[1].find_first_not_of( " 0123456789.e" ) <
			distribution[1].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid nuclear temperature "
			<< distribution[1] );
    {
      double nuclear_temperature;

      std::istringstream iss( distribution[1] );
      Teuchos::extractDataFromISS( iss, nuclear_temperature );

      setQuantity( d_nuclear_temperature, nuclear_temperature );
    }

    TEST_FOR_EXCEPTION( IQT::isnaninf( d_nuclear_temperature ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid nuclear temperature "
			<< d_nuclear_temperature );

    TEST_FOR_EXCEPTION( d_nuclear_temperature <= IQT::zero(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid nuclear temperature "
			<< d_nuclear_temperature );
  }

  // Set the restriction energy
  if( distribution.size() > 2 )
  {
    TEST_FOR_EXCEPTION( distribution[2].find_first_not_of( " 0123456789.e" ) <
			distribution[2].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"construcqted because of an invalid restriction energy "
			<< distribution[2] );
    {
      double restriction_energy;

      std::istringstream iss( distribution[2] );
      Teuchos::extractDataFromISS( iss, restriction_energy );

      setQuantity( d_restriction_energy, restriction_energy );
    }

    TEST_FOR_EXCEPTION( IQT::isnaninf( d_restriction_energy ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid restriction energy "
			<< d_restriction_energy );
  }

  // Set the multiplier
  if( distribution.size() > 3 )
  {
    TEST_FOR_EXCEPTION( distribution[3].find_first_not_of( " 0123456789.e" ) <
			distribution[3].size(),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"construcqted because of an invalid multiplier "
			<< distribution[3] );

    {
      double multiplier;

      std::istringstream iss( distribution[3] );
      Teuchos::extractDataFromISS( iss, multiplier );

      setQuantity( d_multiplier, multiplier );
    }

    TEST_FOR_EXCEPTION( DMQT::isnaninf( d_multiplier ),
			InvalidDistributionStringRepresentation,
			"Error: the Maxwell Fission distribution cannot be "
			"constructed because of an invalid multiplier "
			<< getRawQuantity( d_multiplier ) );
  }

  // Calculate the normalization constant
  this->calculateNormalizationConstant();
}

// Method for testing if two objects are equivalent
template<typename IndependentUnit, typename DependentUnit>
bool UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>::isEqual( const UnitAwareMaxwellFissionDistribution<IndependentUnit,DependentUnit>& other ) const
{
  return d_incident_energy == other.d_incident_energy &&
    d_nuclear_temperature == other.d_nuclear_temperature &&
    d_restriction_energy == other.d_restriction_energy &&
    d_multiplier == other.d_multiplier;
}

} // end Utility namespace

#endif // end UTILITY_MAXWELL_FISSION_DISTRIBUTION_DEF_HPP

//---------------------------------------------------------------------------//
// end Utility_MaxwellFissionDistribution_def.hpp
//---------------------------------------------------------------------------//


/*======================================================================
Copyright 2019 Google LLC

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    https://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
======================================================================*/

#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "Servo_Timer.h"
#include "Home.h"
#include "Caster.h"
#include "Vehicle.h"

#define N_2X2_MATRIX  \
    double N_00 = -cstr[i]->Ns; \
    double N_10 = -cstr[i]->Nt; \
    double N_11 =  cstr[i]->Nt * cstr[i]->Nw;

#define Ni_2X2_MATRIX  \
    double Ni_00 = -1.0/cstr[i]->Ns; \
    double Ni_10 = -1.0/(cstr[i]->Ns * cstr[i]->Nw); \
    double Ni_11 =  1.0/(cstr[i]->Nt * cstr[i]->Nw);

using namespace std;
using namespace Eigen;

// SINGLETON
static Vehicle* gVehicle = NULL;

bool
Vehicle::IsValid() const
{ return gVehicle ? true : false;
}

Vehicle*
Vehicle::HandleGet()
{
  if( gVehicle == NULL )
  {
    if( !( gVehicle = new Vehicle() ) ||
        !gVehicle->IsValid() )
    {
      return NULL;
    }
  }
  return gVehicle;
}

Vehicle::Vehicle()
  :Lambda(M3Z),Mu(V3Z),
   X_veh(0),Y_veh(0),M_veh(0),I_veh(0),num_casters(0),
   L_veh(M3Z),Mu_veh(V3Z),M_gross(0.0),I_gross(0.0),X_gross(0.0),Y_gross(0.0)
{
  hwi_ = HWInterface::HandleGet();

  return;
}

Vehicle::~Vehicle()
{
  // SHOULD SET DAC's TO ZERO HERE,
  // BUT, FOR NOW, LET WATCHDOG DO IT FOR US

  gVehicle = NULL; // SINGLETON

  for( int i=0; i<num_casters; i++ )
  {
    delete cstr[i];
  }
  num_casters = 0;
}

void
Vehicle::Home()
{
  ::Home( cstr[0]->enc_offset,
          cstr[1]->enc_offset,
          cstr[2]->enc_offset,
          cstr[3]->enc_offset);
}

void
Vehicle::Add_Caster( double _Kx, double _Ky, double _ang,
                     double  _b, double  _r,
                     double  _f, double _Mf, double _If,
                     double _Ih, double _Ii, double _Is, double _It, double _Ij,
                     double _Ns, double _Nt, double _Nw,
                     double _px, double _py, double _Mp, double _Ip)
{
  cstr[num_casters] = new Caster( _Kx, _Ky, _ang,
                                   _b,  _r,
                                  _f, _Mf, _If,
                                  _Ih, _Ii, _Is, _It, _Ij,
                                  _Ns, _Nt, _Nw,
                                  _px, _py, _Mp, _Ip);
  num_casters++;

  Add_Gross( _Kx, _Ky, _Mf+_Mp, _Ih+_Ii+_Is+_It+_Ij); // approximate only
}

void
Vehicle::JointRad(Vector8d &jRad)
{
  jRad.setZero();
  return;
  ENC_REGISTER e[8];
  hwi_->EncReadAll(e); // LATCHed by HWInterface

  for( int i=0; i<num_casters; i++ )
  {
    Ni_2X2_MATRIX;

    int j = 2*i;

    jRad[ j ] = -(Ni_00*e[j]               )*ENC_CNT2RAD;
    jRad[j+1] = -(Ni_10*e[j] + Ni_11*e[j+1])*ENC_CNT2RAD;
// NOTE: MINUS SIGN DUE TO DIRECTION OF ENCODERS
// NOTE: ENCODERS GIVE OPPOSITE SIGN OF MOTOR ROTATION

    cstr[i]->a = jRad[j];
    cstr[i]->c = cos(jRad[j]);
    cstr[i]->s = sin(jRad[j]);
  }
}

void
Vehicle::MotorTq(Vector8d const &mtq)
/*** Each motor will produce the specified torque ***/
/*** tq should be a vector of 8 entries in Nm ***/
{
  long counts;

  for( unsigned char j=0; j<2*num_casters; j++ )
  {
    counts =(long)( mtq[j] * (1.0/Nm_PER_AMP) * (1.0/AMPS_PER_VOLT) * COUNTS_PER_VOLT );
    hwi_->RawDAC(j,counts); // RawDAC will check MAX/MIN values
  }
}

void
Vehicle::JointTq(Vector8d const &jtq)
/*** tq should be a vector of 8 entries in Nm ***/
{
  static Vector8d mtq;

  for(int i=0; i<num_casters; i++ )
  {
    Ni_2X2_MATRIX;

    int j = 2*i;
    mtq[ j ] = Ni_00 * jtq[ j ] + Ni_10 * jtq[j+1];
    mtq[j+1] =                    Ni_11 * jtq[j+1];
  }
  MotorTq(mtq);
}

// FOR DATA COLLECTION CONVENIENCE ONLY
void
Vehicle::JtTq2MotAmp(Vector8d const &jtq, Vector8d &motAmp)
{
  static Vector8d mtq;
  for(int i=0; i<num_casters; i++ )
  {
    Ni_2X2_MATRIX;
    int j = 2*i;
    mtq[ j ] = Ni_00 * jtq[ j ] + Ni_10 * jtq[j+1];
    mtq[j+1] =                    Ni_11 * jtq[j+1];
  }
  motAmp = mtq * (1.0/Nm_PER_AMP);
}

// CONSTRAINT MATRIX

void
Vehicle::Fill_C(Matrix3d &C)
{
  for(int i=0; i<num_casters ; i++ )
  {
    double bi = cstr[i]->bi;
    double ri = cstr[i]->ri;
    double c  = cstr[i]->c;
    double s  = cstr[i]->s;
    double Kx = cstr[i]->Kx;
    double Ky = cstr[i]->Ky;

    int j = 2*i;

    C( j ,0) =   bi*s;
    C( j ,1) =  -bi*c;
    C( j ,2) =  -bi*(Kx*c+Ky*s) - 1.0;

    C(j+1,0) =   ri*c;
    C(j+1,1) =   ri*s;
    C(j+1,2) =   ri*(Kx*s-Ky*c);
  }
}

// JACOBIAN VIA C.P.'s to minimize slip in odometry
// USE Jt_cp to minimize contact forces

void
Vehicle::Fill_Jcp(Matrix3x8d &J)
{
  int i;
  static Matrix8x3d  Cp;
  static Matrix3x8d  CptCli;
  static Matrix3d    J;

  for( i=0; i<num_casters; i++ )
  {   // Cli IS 2x2 BLOCK DIAGONAL, SO MULTIPLY
      // IN PIECES TO AVOID SLOW NxN MATRIX OPERATIONS
    double b = cstr[i]->b;
    double r = cstr[i]->r;
    double c = cstr[i]->c;
    double s = cstr[i]->s;
    double Kx= cstr[i]->Kx;
    double Ky= cstr[i]->Ky;

    int j = 2*i;

    // Note: sign of p-dot is: wheel as viewed from ground

    CptCli(0, j ) =  b*s;
    CptCli(0,j+1) =  r*c;
    CptCli(1, j ) = -b*c;
    CptCli(1,j+1) =  r*s;
    CptCli(2, j ) = -b*((Kx*c+Ky*s)+b);
    CptCli(2,j+1) =  r* (Kx*s-Ky*c);

    Cp( j ,0) =  1.0;
    Cp( j ,1) =  0.0;
    Cp( j ,2) = -b*s - Ky;

    Cp(j+1,0) =  0.0;
    Cp(j+1,1) =  1.0;
    Cp(j+1,2) =  b*c + Kx;
  }

  // J = inv(Cpt*Cp)*Cpt * inv(Cl)
  // J = inv(Cpt*Cp)*CptCli
  J = (Cp.transpose()*Cp).llt().solve(CptCli);
}

// JACOBIAN to Minimize Motor Torques

void
Vehicle::Fill_Jt_gamma(Matrix8x3d &Jt)
{
  int i;
  static Matrix8x3d C    = Matrix8x3d::Zero();
  static Matrix8x3d NC   = Matrix8x3d::Zero();
  static Matrix3x8d NCtN = Matrix3x8d::Zero();

  Fill_C( C );

  for( i=0; i<num_casters ; i++ )
  {   // N IS 2x2 BLOCK DIAGONAL, SO MULTIPLY
      // IN PIECES TO AVOID SLOW NxN MATRIX OPERATIONS
    N_2X2_MATRIX;

    int j = 2*i;

    NC( j ,0)   = N_00* C(j,0);
    NC( j ,1)   = N_00* C(j,1);
    NC( j ,2)   = N_00* C(j,2);
    NC(j+1,0)   = N_10* C(j,0) + N_11* C(j+1,0);
    NC(j+1,1)   = N_10* C(j,1) + N_11* C(j+1,1);
    NC(j+1,2)   = N_10* C(j,2) + N_11* C(j+1,2);

    NCtN(0, j ) = NC(j,0)*N_00 + NC(j+1,0)*N_10;
    NCtN(1, j ) = NC(j,1)*N_00 + NC(j+1,1)*N_10;
    NCtN(2, j ) = NC(j,2)*N_00 + NC(j+1,2)*N_10;
    NCtN(0,j+1) =                NC(j+1,0)*N_11;
    NCtN(1,j+1) =                NC(j+1,1)*N_11;
    NCtN(2,j+1) =                NC(j+1,2)*N_11;
  }

  // wdot = Cn xdot ; wdot = N qdot ; qdot = C xdot
  // J_gamma = Cn# N   ; Cn = N C
  // J_gamma  = [(NC)t * NC]i * (NC)t N
  // Jt_gamma = J_gamma.transpose()

  Jt = ( (NC.transpose()*NC).llt().solve(NCtN) ).transpose()

}

// JACOBIAN FOR VIRTUAL LINKAGE AT CONTACT POINTS

void
Vehicle::Fill_E_p(MatrixXd &E)
{
  int i,j;
  double ex, ey, mag_inv;
  double pix, pjx, piy, pjy;

  // DESCRIBE CONNECTIONS OF VIRTUAL TRUSS

//   int pi[NUM_TRUSS_LINKS]={0,1,2,3,1};
//   int pj[NUM_TRUSS_LINKS]={1,2,3,0,3};
  int pi[NUM_TRUSS_LINKS]={0,3,1,2,1,2};
  int pj[NUM_TRUSS_LINKS]={3,1,2,0,0,3};

  for( int k=0; k<NUM_TRUSS_LINKS; k++ )
  {
    i = pi[k];
    j = pj[k];

    pix = cstr[i]->Kx + cstr[i]->b*cstr[i]->c;
    pjx = cstr[j]->Kx + cstr[j]->b*cstr[j]->c;

    piy = cstr[i]->Ky + cstr[i]->b*cstr[i]->s;
    pjy = cstr[j]->Ky + cstr[j]->b*cstr[j]->s;

    ex = pix - pjx;
    ey = piy - pjy;
    mag_inv = 1.0/hypot(ex,ey);
    E(k, 2*i  ) = ex * mag_inv;
    E(k, 2*i+1) = ey * mag_inv;
    E(k, 2*j  ) =  -E(k, 2*i  );
    E(k, 2*j+1) =  -E(k, 2*i+1);
  }

}

void
Vehicle::Fill_E_q(MatrixXd &E)
{
  int j = 2*num_casters;

  MatrixXd Ep(NUM_TRUSS_LINKS,j);
  MatrixXd Cli(j,j);
  // q_dot = Cl * p_dot ==> p_dot = Cli * q_dot
  // eps_dot = Ep * p_dot
  // eps_dot = Ep * Cli * q_dot ==> E_q = Ep * Cli

  Fill_E_p( Ep );
  for(int i=0; i<num_casters; i++ )
  {
    j = 2*i;
    Cli( j , j ) =  cstr[i]->b*cstr[i]->s;
    Cli( j ,j+1) =  cstr[i]->r*cstr[i]->c;

    Cli(j+1, j ) = -cstr[i]->b*cstr[i]->c;
    Cli(j+1,j+1) =  cstr[i]->r*cstr[i]->s;
  }

  E = Ep * Cli;

}

void
Vehicle::Add_Solid(double _x, double _y,
                   double _M, double _I)
{

  // COMPUTE NEW veh QUANTITIES w/ ARGS AND PREVIOUS M & I
  X_veh  = (M_veh * X_veh + _M * _x) / (M_veh + _M);
  Y_veh  = (M_veh * Y_veh + _M * _y) / (M_veh + _M);
  M_veh += _M;  // TOTAL veh MASS
  I_veh += _I +_M*(pow(_x,2) + pow(_y,2));  // new I at (0,0) local coords

  // COMPUTE NEW VEHICLE MASS MATRIX L_veh
  // (ie Lambda_Vehicle for fixed parts not including Casters)
  L_veh(0,0) =  M_veh;
  L_veh(0,1) =  0.0;
  L_veh(0,2) = -M_veh * Y_veh;

  L_veh(1,0) =  0.0;
  L_veh(1,1) =  M_veh;
  L_veh(1,2) =  M_veh * X_veh;

  L_veh(2,0) =  L_veh.at(0,2);
  L_veh(2,1) =  L_veh.at(1,2);
  L_veh(2,2) =  I_veh;

  Add_Gross(_x,_y,_M,_I);
}

void
Vehicle::Add_Gross(double _x, double _y,
                   double _M, double _I)
{
  // COMPUTE NEW ESTIMATES FOR GROSS MASS AND INERTIA (includes Casters)
  X_gross  = (M_gross * X_gross + _M * _x) / (M_gross + _M);
  Y_gross  = (M_gross * Y_gross + _M * _y) / (M_gross + _M);
  M_gross += _M;
  I_gross += _I +_M*(pow(_x,2) + pow(_y,2));  // new I at (0,0) local coords
}

inline void
Vehicle::Fill_Mu_veh( double w )
{
  register double w2 = pow(w,2);

  // (ie Mu_Vehicle for fixed parts not including Casters)
  Mu_veh(0)  = -M_veh * X_veh * w2;
  Mu_veh(1)  = -M_veh * Y_veh * w2;
  Mu_veh(2)  =  0.0;
}

void
Vehicle::Dyn(Vector8d qd, double w)
{
  // INITIALIZE TO VEHICLE PROPERTIES
  Lambda = L_veh;
  Fill_Mu_veh( w );
  Mu = Mu_veh;

  // ADD DYNAMICS FROM THE CASTERS
  for(int i=0; i<num_casters; i++)
  { int j = 2*i;
    cstr[i]->Fill_LM(qd[j],qd[j+1],w);
    Lambda += cstr[i]->Lambda;
    Mu += cstr[i]->Mu;
  }
}

Vector3d
Vehicle::Fill_tqS(Vector8d const &qd, Vector8d const &tq,
                  Vector8d &tqS)
{
  // Experimental function to compute operational force to compensate for
  // rotational friction of the wheel contact patches.

  int i,j;
  double m,b,frd,atq,tqX;
//   double bM[4] = {0.35, 0.57, 0.35, 0.57};
//   double mM[4] = {0.21, 0.17, 0.21, 0.17};
//   double tqM   = 0.35;
  double rdM   = 25.0;
  double bM[4] = {0.20, 0.50, 0.20, 0.50};
  double mM[4] = {0.10, 0.10, 0.10, 0.10};
  double tqM   = 0.60;

  double tqA = 0.0;
  static Vector3d fS = Vector3d::Zero();

  for(i=0; i<num_casters; i++)
  { j = 2*i;
    frd = 1.0 - fabs(qd[j+1])/rdM;
    b = bM[i]*frd;
    m = mM[i]*frd;
    atq = fabs(tq[j]);
    tqX = atq<tqM ? atq/tqM : 1.0;
    tqS[j] = (m*qd[j] + (qd[j]>0?b:-b)) * tqX;
    tqA += tqS[j];
  }

  fS[2] = tqA;
  return fS;
}


//*****************************************************************************
//*****************************************************************************
// Class: CLaricobiusOsakensis
//
//
// Description: the CLaricobiusOsakensis represents a group of LNF insect. scale by m_ScaleFactor
//*****************************************************************************
// 07/07/2021   Rémi Saint-Amant    Creation
//*****************************************************************************

#include "LaricobiusOsakensisEquations.h"
#include "LaricobiusOsakensis.h"
#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/logistic.hpp>

using namespace std;
using namespace WBSF::HOURLY_DATA;
using namespace WBSF::LOF;

namespace WBSF
{

	//*********************************************************************************
	//CLaricobiusOsakensis class

	//*****************************************************************************
	// Object creator
	//
	// Input: See CIndividual creator
	//
	// Note: m_RDR (relative Development Rate)  member is init with random values.
	//*****************************************************************************
	CLaricobiusOsakensis::CLaricobiusOsakensis(CHost* pHost, CTRef creationDate, double age, TSex sex, bool bFertil, size_t generation, double scaleFactor) :
		CIndividual(pHost, creationDate, age, sex, bFertil, generation, scaleFactor)
	{
		//reset creation date
		int year = creationDate.GetYear();
		m_creationDate = GetCreationDate(year);
		m_adult_emergence = GetAdultEmergence(year + 1);

		for (size_t s = 0; s < NB_STAGES; s++)
			m_RDR[s] = Equations().GetRelativeDevRate(s);

		m_t = 0;
		m_Fi = (m_sex == FEMALE) ? Equations().GetFecondity() : 0;
		m_bDeadByAttrition = false;
	}

	CTRef CLaricobiusOsakensis::GetCreationDate(int year)const
	{
		CTRef creationDate;
		double creationCDD = Equations().GetCreationCDD();

		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		//CTRef begin = CTRef(year, JANUARY, DAY_01);
		//CTRef end = CTRef(year, JUNE, DAY_30);

		CTRef begin = CTRef(year, DECEMBER, DAY_01);
		CTRef end = CTRef(year+1, AUGUST, DAY_31);

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !creationDate.IsInit(); TRef++)
		{
			const CWeatherDay& wDay = weather_station.GetDay(TRef);
			double DD = GetStand()->m_DD.GetDD(wDay);
			CDD += DD;

			if (CDD  >= creationCDD)
			{
				creationDate = wDay.GetTRef();
			}
		}
		if (!creationDate.IsInit())
			creationDate = end;//for text only

		ASSERT(creationDate.IsInit());

		return creationDate;
	}

	CTRef CLaricobiusOsakensis::GetAdultEmergence(int year)const
	{
		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTPeriod p = weather_station.GetEntireTPeriod(CTM::DAILY);

		CTRef adult_emergence;
		double adult_emerging_CDD = Equations().GetAdultEmergingCDD();

		CTRef begin = GetStand()->m_diapause_end;
		CTRef end = p.End();
		if (weather_station[year].HaveNext())
			end = min(p.End(), CTRef(begin.GetYear() + 1, JANUARY, DAY_31));

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !adult_emergence.IsInit(); TRef++)
		{
			const CWeatherDay& wday = weather_station.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			double DD = max(0.0, T - Equations().m_EAS[Τᴴ]);
			CDD += DD;
			if (CDD >= adult_emerging_CDD)
			{
				adult_emergence = wday.GetTRef();
			}
		}

		//if (!adult_emergence.IsInit())
			//adult_emergence = CTRef(year, DECEMBER, DAY_31);//pour test seulement a revoir...

		//ASSERT(adult_emergence.IsInit());
		return adult_emergence;
	}

	CLaricobiusOsakensis& CLaricobiusOsakensis::operator=(const CLaricobiusOsakensis& in)
	{
		if (&in != this)
		{
			CIndividual::operator=(in);

			//new relative developement rate
			for (size_t s = 0; s < NB_STAGES; s++)
				m_RDR[s] = Equations().GetRelativeDevRate(s);

			m_dropToGroundDate = in.m_dropToGroundDate;
			m_adult_emergence = in.m_adult_emergence;
			m_reachDate = in.m_reachDate;
			m_t = in.m_t;
			m_Fi = in.m_Fi;
			m_bDeadByAttrition = in.m_bDeadByAttrition;
		}

		return *this;
	}

	//destructor
	CLaricobiusOsakensis::~CLaricobiusOsakensis(void)
	{}

	void CLaricobiusOsakensis::OnNewDay(const CWeatherDay& weather)
	{
		CIndividual::OnNewDay(weather);

		if (weather.GetTRef() == m_creationDate)
		{
			m_age = EGG;
		}
	}

	//*****************************************************************************
	// Develops all stages for one time step
	// Input:	weather: weather of the hour
	//			timeStep: timeStep [h]
	//*****************************************************************************
	void CLaricobiusOsakensis::Live(const CHourlyData& weather, size_t timeStep)
	{
		assert(IsAlive());
		assert(m_status == HEALTHY);

		CLNFHost* pHost = GetHost();
		CLNFStand* pStand = GetStand();

		double nb_steps = (24.0 / timeStep);
		size_t h = weather.GetTRef().GetHour();
		size_t s = GetStage();

		double T = weather[H_TAIR];
		//T = AdjustTLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), T);

		double day_length = weather.GetLocation().GetDayLength(weather.GetTRef()) / 3600.0;//[h]

		if (s < AESTIVAL_DIAPAUSE_ADULT || s == ACTIVE_ADULT)
		{
			//Time step development rate
			double r = Equations().GetRate(s, T) / nb_steps;

			//double corr_r = (s == EGG || s == LARVAE) ? : 1;

			//Relative development rate for this individual
			double rr = m_RDR[s];

			//Time step development rate for this individual
			r *= rr;
			ASSERT(r >= 0 && r < 1);

			//Adjust age
			m_age += r;

			if (!m_dropToGroundDate.IsInit() && m_age > LARVAE4 + 0.9)//drop to the soil when 90% competed (guess)
				m_dropToGroundDate = weather.GetTRef().as(CTM::DAILY);

			//evaluate attrition once a day
			if (GetStand()->m_bApplyAttrition)
			{
				if (IsDeadByAttrition(s, T, r))
					m_bDeadByAttrition = true;
			}

		}
		else if (s == AESTIVAL_DIAPAUSE_ADULT)
		{
			CTRef TRef = weather.GetTRef().as(CTM::DAILY);
			if (TRef == m_adult_emergence)
				m_age = ACTIVE_ADULT;
		}

		if (m_sex == FEMALE && GetStage() >= ACTIVE_ADULT)
		{
			double to = 0;
			double t = timeStep / 24.0;
			double λ = Equations().GetFecondityRate(GetAge(), weather[H_TAIR]);
			double brood = m_Fi * (exp(-λ * (m_t - to)) - exp(-λ * (m_t + t - to)));

			m_broods += brood;
			m_totalBroods += brood;

			m_t += t;
		}
		//else//ACTIVE_ADULT
		//{
		//	double r = (1.0 / m_adult_longevity) / nb_steps;
		//	ASSERT(r >= 0 && r < 1);

		//	m_age += r;
		//}
	}

	//*****************************************************************************
	// Develops all stages, including adults
	// Input:	weather: the weather of the day
	//*****************************************************************************
	void CLaricobiusOsakensis::Live(const CWeatherDay& weather)
	{
		CIndividual::Live(weather);

		ASSERT(IsCreated(weather.GetTRef()));

		if (!IsCreated(weather.GetTRef()))
			return;

		size_t nbSteps = GetTimeStep().NbSteps();
		for (size_t step = 0; step < nbSteps&&IsAlive(); step++)
		{
			size_t h = step * GetTimeStep();
			Live(weather[h], GetTimeStep());
		}

		if (weather.GetTRef() == m_creationDate || HasChangedStage())
			m_reachDate[GetStage()] = weather.GetTRef();

	}

	void CLaricobiusOsakensis::Brood(const CWeatherDay& weather)
	{
		assert(/*IsAlive() &&*/ m_sex == FEMALE);

		if (GetStage() == ACTIVE_ADULT)
		{
			//no brood process done

			//brooding
			//m_broods = m_F;
			//m_totalBroods = m_F;
			//m_F = 0;
			//m_F -= m_F * r;
		}
	}

	// kills by old age and frost
	// Output:  Individual's state is updated to follow update
	void CLaricobiusOsakensis::Die(const CWeatherDay& weather)
	{
		//attrition mortality. Killed at the end of time step

		if (m_bDeadByAttrition)
		{
			m_status = DEAD;
			m_death = ATTRITION;
		}
		else if (GetStage() == DEAD_ADULT)
		{
			//Old age
			m_status = DEAD;
			m_death = OLD_AGE;
		}
		else
		{
			//size_t s = GetStage();

			////Preliminary assessment of the cold tolerance of Laricobius Osakensis, a winter - active predator of the hemlock woolly adelgid from western canada
			////Leland M.Humble
			//static const double COLD_TOLERENCE_T[NB_STAGES] = { -27.5,-22.1, -99.0,-99.0,-19.0,-19.0 };
			////Toland:L. Osakensis was -13.6 oC (± 0.5) with temperatures that ranged from -6 oC to -21 oC.
			//if (weather[H_TMIN][MEAN] < COLD_TOLERENCE_T[s])
			//{
			//	m_status = DEAD;
			//	m_death = FROZEN;
			//}
		}
	}

	//s: stage
	//T: temperature for this time step
	//r: devlopement rate for this time step
	bool CLaricobiusOsakensis::IsDeadByAttrition(size_t s, double T, double r)const
	{
		bool bDeath = false;

		//daily survival
		double ds = GetStand()->m_equations.GetDailySurvivalRate(s, T);

		//time step survival
		double S = pow(ds, r);

		//Computes attrition (probability of survival in a given time step, based on development rate)
		if (RandomGenerator().RandUniform() > S)
			bDeath = true;

		return bDeath;
	}

	//*****************************************************************************
	// GetStat gather information of this object
	//
	// Input: stat: the statistic object
	// Output: The stat is modified
	//*****************************************************************************
	void CLaricobiusOsakensis::GetStat(CTRef d, CModelStat& stat)
	{
		if (IsCreated(d))
		{
			size_t s = GetStage();
			ASSERT(s <= DEAD_ADULT);

			if (IsAlive() || (s == DEAD_ADULT))
				stat[S_EGG + s] += m_scaleFactor;

			stat[S_LARVAE] = stat[S_L1]+ stat[S_L2]+ stat[S_L3]+ stat[S_L4];

			if (HasChangedStatus() && m_status == DEAD && m_death == ATTRITION)
				stat[S_DEAD_ATTRTION] += m_scaleFactor;

			if (HasChangedStage())
				stat[S_M_EGG + s] += m_scaleFactor;

			//if (s == ACTIVE_ADULT)
			//{
			//	stat[S_ADULT_ABUNDANCE] += m_scaleFactor * m_adult_abundance;
			//}
		}
	}

	void CLaricobiusOsakensis::Pack(const CIndividualPtr& pBug)
	{
		assert(m_sex == pBug->GetSex());

		CLaricobiusOsakensis* in = (CLaricobiusOsakensis*)(pBug.get());
		CIndividual::Pack(pBug);
	}

	double CLaricobiusOsakensis::GetInstar(bool includeLast)const
	{
		return (IsAlive() || m_death == OLD_AGE) ? GetStage() : CBioSIMModelBase::VMISS;
	}

	//*********************************************************************************************************************

	//*********************************************************************************
	//CLNFHost

	CLNFHost::CLNFHost(CStand* pStand) :
		CHost(pStand)
	{
	}

	void CLNFHost::Live(const CWeatherDay& weather)
	{
		CHost::Live(weather);
	}

	void CLNFHost::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CHost::GetStat(d, stat, generation);
	}

	//*************************************************
	//CLNFStand

	void CLNFStand::init(int year, const CWeatherYears& weather)
	{
		m_diapause_end = ComputeDiapauseEnd(weather[year]);
	}

	CTRef CLNFStand::ComputeDiapauseEnd(const CWeatherYear& weather)const
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM::DAILY);
		//CTPeriod p = weather.GetPrevious().GetEntireTPeriod(CTM::DAILY);

		double sumDD = 0;

		for (size_t ii = (m_equations.m_ADE[ʎ0] - 1); ii <= (m_equations.m_ADE[ʎ1] - 1); ii++)
		{
			CTRef TRef = p.Begin() + ii;
			const CWeatherDay& wday = weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			double DD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative
			sumDD += DD;
		}

		boost::math::logistic_distribution<double> diapause_end_dist(m_equations.m_ADE[ʎ2], m_equations.m_ADE[ʎ3]);
		int begin = (int)Round((m_equations.m_ADE[ʎ1] - 1) + m_equations.m_ADE[ʎa] * cdf(diapause_end_dist, sumDD), 0);

		return p.Begin() + begin;
	}

	void CLNFStand::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CStand::GetStat(d, stat, generation);

		const CWeatherStation& weather_station = GetModel()->m_weather;
		const CWeatherDay& wday = weather_station.GetDay(d);

		//use year of diapause to compute correctly the adult emergence cdd
		int year = m_diapause_end.GetYear();

		CTRef begin = CTRef(year, JANUARY, DAY_01);
		CTRef end = CTRef(year, DECEMBER, DAY_31);

		if (d >= begin && d <= end)
		{
			//Egg creation DD (allen 1976)
			//m_egg_creation_CDD += m_DD.GetDD(wday);
			//stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;

			//diapause end negative DD
			double T = wday[H_TNTX][MEAN];
			//T = max(m_equations.m_ADE[ʎa], T);
			double NDD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative

			int ii = d - begin;
			if (ii >= int(m_equations.m_ADE[ʎ0] - 1) && ii <= int(m_equations.m_ADE[ʎ1] - 1))
				m_diapause_end_NCDD += NDD;

			stat[S_DIAPAUSE_END_NCDD] = m_diapause_end_NCDD;
		}

		begin = m_diapause_end;
		end = CTRef(m_diapause_end.GetYear() + 1, JANUARY, DAY_31);
		if (d >= begin && d <= end)
		{
			//adult emergence (growing DD)
			double T = wday[H_TNTX][MEAN];
			double GDD = max(0.0, T - m_equations.m_EAS[Τᴴ]);
			m_adult_emergence_CDD += GDD;
			stat[S_ADULT_EMERGENCE_CDD] = m_adult_emergence_CDD;
		}

		//compute egg creation
		begin = CTRef(year-1, DECEMBER, DAY_01);
		end = CTRef(year, AUGUST, DAY_31);

		if (d >= begin && d <= end)
		{
			//Egg creation DD (allen 1976)
			m_egg_creation_CDD += m_DD.GetDD(wday);
			stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;
		}

	}

}