#include <catch2/catch.hpp>

#include "constitutive/internal_variables.hpp"
#include "constitutive/mechanics/solid/small_strain_J2_plasticity.hpp"
#include "constitutive/mechanics/detail/J2_plasticity.hpp"
#include "constitutive/constitutive_model_factory.hpp"
#include "material/isotropic_elastic_plastic.hpp"

#include "exceptions.hpp"
#include "numeric/dense_matrix.hpp"
#include "io/json.hpp"

#include <Eigen/Eigenvalues>

std::string json_input_file()
{
    return "{\"name\": \"rubber\", \"elastic_modulus\": 2.0, \"poissons_ratio\": 0.45}";
}

constexpr auto internal_variable_size = 2;
constexpr auto ZERO_MARGIN = 1.0e-5;

using namespace neon;
using neon::json;

TEST_CASE("Plane stress linear elasticity factory error")
{
    using namespace neon::mechanics::plane;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    REQUIRE_THROWS_AS(make_constitutive_model(variables,
                                              json::parse("{\"Badkey\" : \"donkey\"}"),
                                              json::parse("{\"constitutive\" : {\"name\" : "
                                                          "\"plane_strain\"}}")),
                      std::domain_error);
}
TEST_CASE("Plane stress elasticity")
{
    using namespace neon::mechanics::plane;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::displacement_gradient,
                   variable::second::cauchy_stress,
                   variable::scalar::DetF);

    auto elastic_model = make_constitutive_model(variables,
                                                 json::parse("{\"name\": \"steel\", "
                                                             "\"elastic_modulus\": 200.0e3, "
                                                             "\"poissons_ratio\": 0.3}"),
                                                 json::parse("{\"constitutive\" : {\"name\" : "
                                                             "\"plane_stress\"}}"));

    // Get the tensor variables
    auto [displacement_gradients,
          cauchy_stresses] = variables->get(variable::second::displacement_gradient,
                                            variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    for (auto& H : displacement_gradients) H = internal_variables_t::second_tensor_type::Zero();
    for (auto& J : J_list) J = 1.0;

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(elastic_model->is_symmetric());
        REQUIRE(elastic_model->is_finite_deformation() == false);
        REQUIRE(elastic_model->intrinsic_material().name() == "steel");

        REQUIRE(variables->has(variable::scalar::von_mises_stress));
        REQUIRE(variables->has(variable::second::cauchy_stress));
        REQUIRE(variables->has(variable::second::linearised_strain));
        REQUIRE(variables->has(variable::fourth::tangent_operator));
    }
    SECTION("No load")
    {
        elastic_model->update_internal_variables(1.0);

        // Ensure symmetry is correct
        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE(material_tangent(0, 0) == Approx(219780.21978022));
            REQUIRE(material_tangent(1, 1) == Approx(219780.21978022));
            REQUIRE(material_tangent(2, 2) == Approx(76923.0769230769));
            REQUIRE(material_tangent(0, 1) == Approx(65934.065934066));
            REQUIRE(material_tangent(1, 0) == Approx(65934.065934066));

            REQUIRE(material_tangent(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 0) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 1) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("Uniaxial elastic load")
    {
        for (auto& H : displacement_gradients) H(1, 1) = 0.001;

        elastic_model->update_internal_variables(1.0);

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE(material_tangent(0, 0) == Approx(219780.21978022));
            REQUIRE(material_tangent(1, 1) == Approx(219780.21978022));
            REQUIRE(material_tangent(2, 2) == Approx(76923.0769230769));
            REQUIRE(material_tangent(0, 1) == Approx(65934.065934066));
            REQUIRE(material_tangent(1, 0) == Approx(65934.065934066));

            REQUIRE(material_tangent(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 0) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 1) == Approx(0.0).margin(ZERO_MARGIN));

            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto const& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) > 0.0);
            REQUIRE(cauchy_stress(1, 1) > 0.0);
        }

        for (auto& von_mises_stress : von_mises_stresses)
        {
            REQUIRE(von_mises_stress < 200.0e6);
        }
    }
}
TEST_CASE("Plane strain elasticity")
{
    using namespace neon::mechanics::plane;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::displacement_gradient,
                   variable::second::cauchy_stress,
                   variable::scalar::DetF);

    auto elastic_model = make_constitutive_model(variables,
                                                 json::parse("{\"name\": \"steel\", "
                                                             "\"elastic_modulus\": 200.0e3, "
                                                             "\"poissons_ratio\": 0.3}"),
                                                 json::parse("{\"constitutive\" : {\"name\" : "
                                                             "\"plane_strain\"}}"));

    // Get the tensor variables
    auto [displacement_gradients,
          cauchy_stresses] = variables->get(variable::second::displacement_gradient,
                                            variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    for (auto& H : displacement_gradients) H = internal_variables_t::second_tensor_type::Zero();
    for (auto& J : J_list) J = 1.0;

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(elastic_model->is_symmetric());
        REQUIRE(elastic_model->is_finite_deformation() == false);
        REQUIRE(elastic_model->intrinsic_material().name() == "steel");

        REQUIRE(variables->has(variable::scalar::von_mises_stress));
        REQUIRE(variables->has(variable::second::cauchy_stress));
        REQUIRE(variables->has(variable::second::linearised_strain));
        REQUIRE(variables->has(variable::fourth::tangent_operator));
    }
    SECTION("No load")
    {
        elastic_model->update_internal_variables(1.0);

        // Ensure symmetry is correct
        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE(material_tangent(0, 0) == Approx(269230.769230769));
            REQUIRE(material_tangent(1, 1) == Approx(269230.769230769));
            REQUIRE(material_tangent(2, 2) == Approx(76923.0769230769));
            REQUIRE(material_tangent(0, 1) == Approx(115384.615384615));

            REQUIRE(material_tangent(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 0) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 1) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("Uniaxial elastic load")
    {
        for (auto& H : displacement_gradients) H(1, 1) = 0.001;

        elastic_model->update_internal_variables(1.0);

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE(material_tangent(0, 0) == Approx(269230.769230769));
            REQUIRE(material_tangent(1, 1) == Approx(269230.769230769));
            REQUIRE(material_tangent(2, 2) == Approx(76923.0769230769));
            REQUIRE(material_tangent(0, 1) == Approx(115384.615384615));

            REQUIRE(material_tangent(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 0) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 1) == Approx(0.0).margin(ZERO_MARGIN));

            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto const& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) > 0.0);
            REQUIRE(cauchy_stress(1, 1) > 0.0);
        }

        for (auto& von_mises_stress : von_mises_stresses)
        {
            REQUIRE(von_mises_stress < 200.0e6);
        }
    }
}
TEST_CASE("Solid mechanics elasticity model")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::displacement_gradient,
                   variable::second::cauchy_stress,
                   variable::scalar::DetF);

    auto elastic_model = make_constitutive_model(variables,
                                                 json::parse("{\"name\": \"steel\", "
                                                             "\"elastic_modulus\": 200.0e3, "
                                                             "\"poissons_ratio\": 0.3}"),
                                                 json::parse("{\"constitutive\" : {\"name\" : "
                                                             "\"isotropic_linear_elasticity\"}}"));

    // Get the tensor variables
    auto [displacement_gradients,
          cauchy_stresses] = variables->get(variable::second::displacement_gradient,
                                            variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    for (auto& H : displacement_gradients) H = internal_variables_t::second_tensor_type::Zero();
    for (auto& J : J_list) J = 1.0;

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(elastic_model->is_symmetric());
        REQUIRE(elastic_model->is_finite_deformation() == false);
        REQUIRE(elastic_model->intrinsic_material().name() == "steel");

        REQUIRE(variables->has(variable::scalar::von_mises_stress));
        REQUIRE(variables->has(variable::second::cauchy_stress));
        REQUIRE(variables->has(variable::second::linearised_strain));
        REQUIRE(variables->has(variable::fourth::tangent_operator));
    }
    SECTION("No load")
    {
        elastic_model->update_internal_variables(1.0);

        // Ensure symmetry is correct
        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE(material_tangent(0, 0) == Approx(269230.769230769));
            REQUIRE(material_tangent(1, 1) == Approx(269230.769230769));
            REQUIRE(material_tangent(2, 2) == Approx(269230.769230769));

            REQUIRE(material_tangent(3, 3) == Approx(76923.0769230769));
            REQUIRE(material_tangent(4, 4) == Approx(76923.0769230769));
            REQUIRE(material_tangent(5, 5) == Approx(76923.0769230769));

            REQUIRE(material_tangent(0, 1) == Approx(115384.615384615));
            REQUIRE(material_tangent(0, 2) == Approx(115384.615384615));
            REQUIRE(material_tangent(1, 2) == Approx(115384.615384615));

            REQUIRE(material_tangent(0, 3) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(0, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(0, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(1, 3) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(2, 3) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(3, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(3, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(4, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("Uniaxial elastic load")
    {
        for (auto& H : displacement_gradients) H(1, 1) = 0.001;

        elastic_model->update_internal_variables(1.0);

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE(material_tangent(0, 0) == Approx(269230.769230769));
            REQUIRE(material_tangent(1, 1) == Approx(269230.769230769));
            REQUIRE(material_tangent(2, 2) == Approx(269230.769230769));

            REQUIRE(material_tangent(3, 3) == Approx(76923.0769230769));
            REQUIRE(material_tangent(4, 4) == Approx(76923.0769230769));
            REQUIRE(material_tangent(5, 5) == Approx(76923.0769230769));

            REQUIRE(material_tangent(0, 1) == Approx(115384.615384615));
            REQUIRE(material_tangent(0, 2) == Approx(115384.615384615));
            REQUIRE(material_tangent(1, 2) == Approx(115384.615384615));

            REQUIRE(material_tangent(0, 3) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(0, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(0, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(1, 3) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(1, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(2, 3) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(2, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(3, 4) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(material_tangent(3, 5) == Approx(0.0).margin(ZERO_MARGIN));

            REQUIRE(material_tangent(4, 5) == Approx(0.0).margin(ZERO_MARGIN));

            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto const& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) > 0.0);
            REQUIRE(cauchy_stress(1, 1) > 0.0);
        }

        for (auto& von_mises_stress : von_mises_stresses)
        {
            REQUIRE(von_mises_stress < 200.0e6);
        }
    }
}
TEST_CASE("Solid mechanics J2 plasticity model factory errors")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    SECTION("finite_strain not specified")
    {
        REQUIRE_THROWS_AS(make_constitutive_model(variables,
                                                  json::parse("{}"),
                                                  json::parse("{\"constitutive\" : {\"name\" "
                                                              ": "
                                                              "\"J2_plasticity\"}}")),
                          std::domain_error);
    }
    SECTION("Damage model not specified correctly")
    {
        REQUIRE_THROWS_AS(make_constitutive_model(variables,
                                                  json::parse("{}"),
                                                  json::parse("{\"constitutive\" : {\"name\" "
                                                              ": "
                                                              "\"J2_plasticity\", "
                                                              "\"damage\" : "
                                                              "\"isotropic_chaboche\", "
                                                              "\"finite_strain\" : true}}")),
                          std::domain_error);
    }
}
TEST_CASE("Solid mechanics J2 plasticity")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::displacement_gradient, variable::second::cauchy_stress);
    variables->add(variable::scalar::DetF);

    auto const material_data = json::parse("{\"name\": \"steel\", "
                                           "\"elastic_modulus\": 200.0e9, "
                                           "\"poissons_ratio\": 0.3, "
                                           "\"yield_stress\": 200.0e6, "
                                           "\"isotropic_hardening_modulus\": 400.0e6}");

    auto const constitutive_data = json::parse("{\"constitutive\" : "
                                               "{\"name\" : \"J2_plasticity\", "
                                               "\"finite_strain\" : false}}");

    auto small_strain_J2_plasticity = make_constitutive_model(variables,
                                                              material_data,
                                                              constitutive_data);

    // Get the tensor variables
    auto [displacement_gradients,
          cauchy_stresses] = variables->get(variable::second::displacement_gradient,
                                            variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    for (auto& H : displacement_gradients) H = neon::matrix3::Zero();
    for (auto& J : J_list) J = 1.0;

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(small_strain_J2_plasticity->is_symmetric());
        REQUIRE(small_strain_J2_plasticity->is_finite_deformation() == false);
        REQUIRE(small_strain_J2_plasticity->intrinsic_material().name() == "steel");

        REQUIRE(variables->has(variable::scalar::von_mises_stress));
        REQUIRE(variables->has(variable::scalar::effective_plastic_strain));
        REQUIRE(variables->has(variable::second::linearised_strain));
        REQUIRE(variables->has(variable::second::linearised_plastic_strain));
        REQUIRE(variables->has(variable::fourth::tangent_operator));
    }
    SECTION("Helper function")
    {
        neon::isotropic_elastic_plastic material{material_data};

        REQUIRE(neon::mechanics::evaluate_J2_yield_function(material, 100.0e6, 0.0) <= 0.0);
        REQUIRE(neon::mechanics::evaluate_J2_yield_function(material, 300.0e6, 0.0) > 0.0);
    }
    SECTION("No load")
    {
        small_strain_J2_plasticity->update_internal_variables(1.0);

        // Ensure symmetry is correct
        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("Uniaxial elastic load")
    {
        for (auto& H : displacement_gradients) H(2, 2) = 0.001;

        small_strain_J2_plasticity->update_internal_variables(1.0);

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& material_tangent : material_tangents)
        {
            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto const& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) > 0.0);
            REQUIRE(cauchy_stress(1, 1) > 0.0);
            REQUIRE(cauchy_stress(2, 2) > 0.0);
        }

        for (auto& accumulated_plastic_strain : accumulated_plastic_strains)
        {
            REQUIRE(accumulated_plastic_strain == Approx(0.0).margin(ZERO_MARGIN));
        }

        for (auto& von_mises_stress : von_mises_stresses)
        {
            REQUIRE(von_mises_stress < 200.0e6);
        }
    }
    SECTION("Plastic uniaxial elastic load")
    {
        for (auto& H : displacement_gradients) H(2, 2) = 0.003;

        small_strain_J2_plasticity->update_internal_variables(1.0);

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& material_tangent : material_tangents)
        {
            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) > 0.0);
            REQUIRE(cauchy_stress(1, 1) > 0.0);
            REQUIRE(cauchy_stress(2, 2) > 0.0);
        }

        for (auto& accumulated_plastic_strain : accumulated_plastic_strains)
        {
            REQUIRE(accumulated_plastic_strain > 0.0);
        }

        for (auto& von_mises_stress : von_mises_stresses)
        {
            // Should experience hardening
            REQUIRE(von_mises_stress <= 201.0e6);
            REQUIRE(von_mises_stress > 200.0e6);
        }
    }
}
TEST_CASE("Solid mechanics J2 plasticity damage")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    variables->add(variable::second::displacement_gradient, variable::second::cauchy_stress);
    variables->add(variable::scalar::DetF);

    auto const material_input{"{\"name\":\"steel\","
                              "\"elastic_modulus\":134.0e3,"
                              "\"poissons_ratio\":0.3,"
                              "\"yield_stress\":85,"
                              "\"kinematic_hardening_modulus\": 5500,"
                              "\"softening_multiplier\" : 250,"
                              "\"plasticity_viscous_exponent\" : 2.5,"
                              "\"plasticity_viscous_denominator\" : 1220,"
                              "\"damage_viscous_exponent\" : 2,"
                              "\"damage_viscous_denominator\" : 0.6"
                              "}"};

    auto const constitutive_input{"{\"constitutive\" : "
                                  "{\"name\" : \"J2_plasticity\","
                                  "\"damage\" : \"isotropic_chaboche\", "
                                  "\"finite_strain\" : false}}"};

    auto small_strain_J2_plasticity_damage = make_constitutive_model(variables,
                                                                     json::parse(material_input),
                                                                     json::parse(constitutive_input));

    // Get the tensor variables
    auto [displacement_gradients,
          cauchy_stresses] = variables->get(variable::second::displacement_gradient,
                                            variable::second::cauchy_stress);

    auto [J_list, damage_list] = variables->get(variable::scalar::DetF, variable::scalar::damage);

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    for (auto& H : displacement_gradients) H = neon::matrix3::Zero();
    for (auto& J : J_list) J = 1.0;

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(small_strain_J2_plasticity_damage->is_finite_deformation() == false);
        REQUIRE(small_strain_J2_plasticity_damage->is_symmetric() == false);

        REQUIRE(small_strain_J2_plasticity_damage->intrinsic_material().name() == "steel");

        REQUIRE(variables->has(variable::scalar::von_mises_stress));
        REQUIRE(variables->has(variable::scalar::effective_plastic_strain));
        REQUIRE(variables->has(variable::second::linearised_strain));
        REQUIRE(variables->has(variable::second::linearised_plastic_strain));
        REQUIRE(variables->has(variable::fourth::tangent_operator));
        REQUIRE(variables->has(variable::scalar::damage));
        REQUIRE(variables->has(variable::scalar::energy_release_rate));
        REQUIRE(variables->has(variable::second::kinematic_hardening));
        REQUIRE(variables->has(variable::second::back_stress));
    }
    SECTION("Uniaxial elastic load")
    {
        for (auto& H : displacement_gradients) H(2, 2) = 0.0008;

        small_strain_J2_plasticity_damage->update_internal_variables(1.0);

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& material_tangent : material_tangents)
        {
            // Ensure symmetry is correct when the loading is elastic
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) == Approx(cauchy_stress(1, 1)));
            REQUIRE(cauchy_stress(1, 1) > 0.0);
            REQUIRE(cauchy_stress(2, 2) > 0.0);
        }
        for (auto& accumulated_plastic_strain : accumulated_plastic_strains)
        {
            REQUIRE(accumulated_plastic_strain == Approx(0.0).margin(ZERO_MARGIN));
        }
        for (auto& von_mises_stress : von_mises_stresses)
        {
            REQUIRE(von_mises_stress > 80);
            REQUIRE(von_mises_stress < 85);
        }
    }
    SECTION("Plastic uniaxial load")
    {
        for (auto& H : displacement_gradients) H(2, 2) = 0.0009; // 0.000825 for comparison with 1D

        small_strain_J2_plasticity_damage->update_internal_variables(
            0.01); // time here is real (not pseudo time)

        auto [von_mises_stresses,
              accumulated_plastic_strains] = variables->get(variable::scalar::von_mises_stress,
                                                            variable::scalar::effective_plastic_strain);

        for (auto const& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() != Approx(0.0).margin(ZERO_MARGIN));

            // Shear components should be close to zero
            REQUIRE(cauchy_stress(0, 1) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(1, 2) == Approx(0.0).margin(ZERO_MARGIN));
            REQUIRE(cauchy_stress(0, 0) > 0.0);
            REQUIRE(cauchy_stress(1, 1) > 0.0);
            REQUIRE(cauchy_stress(2, 2) > 0.0);
        }
        for (auto& accumulated_plastic_strain : accumulated_plastic_strains)
        {
            REQUIRE(accumulated_plastic_strain > 0.0);
        }
        for (auto& damage_var : damage_list)
        {
            REQUIRE(damage_var > 0.0);
        }
        for (auto& von_mises_stress : von_mises_stresses)
        {
            // Should experience hardening
            REQUIRE(von_mises_stress <= 100);
            REQUIRE(von_mises_stress > 85);
        }
        for (auto const& material_tangent : material_tangents)
        {
            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }
    }
}


#ifndef _PLAN_CONTAINER_H_
#define _PLAN_CONTAINER_H_

#include <Eigen/Eigen>
#include <vector>
#include <ros/ros.h>

#include <bspline_opt/uniform_bspline.h>
#include <traj_utils/polynomial_traj.h>

using std::vector;

namespace ego_planner
{

  class GlobalTrajData
  {
  private:
  public:
    PolynomialTraj global_traj_;
    vector<UniformBspline> local_traj_;

    double global_duration_;
    ros::Time global_start_time_;
    double local_start_time_, local_end_time_;
    double time_increase_;
    double last_time_inc_;
    double last_progress_time_;

    GlobalTrajData(/* args */) {}

    ~GlobalTrajData() {}

    bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.1; }

    void setGlobalTraj(const PolynomialTraj &traj, const ros::Time &time)
    {
      global_traj_ = traj;
      global_traj_.init();
      global_duration_ = global_traj_.getTimeSum();
      global_start_time_ = time;

      local_traj_.clear();
      local_start_time_ = -1;
      local_end_time_ = -1;
      time_increase_ = 0.0;
      last_time_inc_ = 0.0;
      last_progress_time_ = 0.0;
    }

    void setLocalTraj(UniformBspline traj, double local_ts, double local_te, double time_inc)
    {
      local_traj_.resize(3);
      local_traj_[0] = traj;
      local_traj_[1] = local_traj_[0].getDerivative();
      local_traj_[2] = local_traj_[1].getDerivative();

      local_start_time_ = local_ts;
      local_end_time_ = local_te;
      global_duration_ += time_inc;
      time_increase_ += time_inc;
      last_time_inc_ = time_inc;
    }

    Eigen::Vector3d getPosition(double t)
    {
      if (t >= -1e-3 && t <= local_start_time_)
      {
        return global_traj_.evaluate(t - time_increase_ + last_time_inc_);
      }
      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
      {
        return global_traj_.evaluate(t - time_increase_);
      }
      else
      {
        double tm, tmp;
        local_traj_[0].getTimeSpan(tm, tmp);
        return local_traj_[0].evaluateDeBoor(tm + t - local_start_time_);
      }
    }

    Eigen::Vector3d getVelocity(double t)
    {
      if (t >= -1e-3 && t <= local_start_time_)
      {
        return global_traj_.evaluateVel(t);
      }
      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
      {
        return global_traj_.evaluateVel(t - time_increase_);
      }
      else
      {
        double tm, tmp;
        local_traj_[0].getTimeSpan(tm, tmp);
        return local_traj_[1].evaluateDeBoor(tm + t - local_start_time_);
      }
    }

    Eigen::Vector3d getAcceleration(double t)
    {
      if (t >= -1e-3 && t <= local_start_time_)
      {
        return global_traj_.evaluateAcc(t);
      }
      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
      {
        return global_traj_.evaluateAcc(t - time_increase_);
      }
      else
      {
        double tm, tmp;
        local_traj_[0].getTimeSpan(tm, tmp);
        return local_traj_[2].evaluateDeBoor(tm + t - local_start_time_);
      }
    }

    // get Bspline paramterization data of a local trajectory within a sphere
    // start_t: start time of the trajectory
    // dist_pt: distance between the discretized points
    void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt,
                         vector<Eigen::Vector3d> &point_set, vector<Eigen::Vector3d> &start_end_derivative,
                         double &dt, double &seg_duration)
    {
      double seg_length = 0.0; // length of the truncated segment
      double seg_time = 0.0;   // duration of the truncated segment
      double radius = 0.0;     // distance to the first point of the segment

      double delt = 0.2;
      Eigen::Vector3d first_pt = getPosition(start_t); // first point of the segment
      Eigen::Vector3d prev_pt = first_pt;              // previous point
      Eigen::Vector3d cur_pt;                          // current point

      // go forward until the traj exceed radius or global time

      while (radius < des_radius && seg_time < global_duration_ - start_t - 1e-3)
      {
        seg_time += delt;
        seg_time = min(seg_time, global_duration_ - start_t);

        cur_pt = getPosition(start_t + seg_time);
        seg_length += (cur_pt - prev_pt).norm();
        prev_pt = cur_pt;
        radius = (cur_pt - first_pt).norm();
      }

      // get parameterization dt by desired density of points
      int seg_num = floor(seg_length / dist_pt);

      // get outputs

      seg_duration = seg_time; // duration of the truncated segment
      dt = seg_time / seg_num; // time difference between two points

      for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt)
      {
        cur_pt = getPosition(start_t + tp);
        point_set.push_back(cur_pt);
      }

      start_end_derivative.push_back(getVelocity(start_t));
      start_end_derivative.push_back(getVelocity(start_t + seg_time));
      start_end_derivative.push_back(getAcceleration(start_t));
      start_end_derivative.push_back(getAcceleration(start_t + seg_time));
    }

    // get Bspline paramterization data of a fixed duration local trajectory
    // start_t: start time of the trajectory
    // duration: time length of the segment
    // seg_num: discretized the segment into *seg_num* parts
    void getTrajByDuration(double start_t, double duration, int seg_num,
                           vector<Eigen::Vector3d> &point_set,
                           vector<Eigen::Vector3d> &start_end_derivative, double &dt)
    {
      dt = duration / seg_num;
      Eigen::Vector3d cur_pt;
      for (double tp = 0.0; tp <= duration + 1e-4; tp += dt)
      {
        cur_pt = getPosition(start_t + tp);
        point_set.push_back(cur_pt);
      }

      start_end_derivative.push_back(getVelocity(start_t));
      start_end_derivative.push_back(getVelocity(start_t + duration));
      start_end_derivative.push_back(getAcceleration(start_t));
      start_end_derivative.push_back(getAcceleration(start_t + duration));
    }
  };

  struct PlanParameters
  {
    /* planning algorithm parameters */
    double max_vel_, max_acc_, max_jerk_; // physical limits
    double ctrl_pt_dist;                  // distance between adjacient B-spline control points
    double feasibility_tolerance_;        // permitted ratio of vel/acc exceeding limits
    double planning_horizen_;
    bool use_distinctive_trajs;
    int drone_id; // single drone: drone_id <= -1, swarm: drone_id >= 0

    /* processing time */
    double time_search_ = 0.0;
    double time_optimize_ = 0.0;
    double time_adjust_ = 0.0;
  };

  struct LocalTrajData
  {
    /* info of generated traj */

    int traj_id_;
    double duration_;
    ros::Time start_time_;
    Eigen::Vector3d start_pos_;
    UniformBspline position_traj_, velocity_traj_, acceleration_traj_;
  };

  struct OneTrajDataOfSwarm
  {
    /* info of generated traj */

    int drone_id;
    double duration_;
    ros::Time start_time_;
    Eigen::Vector3d start_pos_;
    UniformBspline position_traj_;
  };

  typedef std::vector<OneTrajDataOfSwarm> SwarmTrajData;

} // namespace ego_planner

#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_OccupationNumberEvaluator.cpp
//! \author Alex Robinson
//! \brief  The occupation number evaluator definition
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

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

// Teuchos Includes
#include <Teuchos_ScalarTraits.hpp>

// FRENSIE Includes
#include "DataGen_OccupationNumberEvaluator.hpp"
#include "MonteCarlo_ComptonProfileHelpers.hpp"
#include "Utility_SortAlgorithms.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace DataGen{

// Constructor
/*! \details The full electron momentum grid and Compton profile should
 * be given.
 */
OccupationNumberEvaluator::OccupationNumberEvaluator(
		   const Teuchos::Array<double>& electron_momentum_projections,
		   const Teuchos::Array<double>& compton_profile,
		   const double norm_constant_precision )
  : d_compton_profile_norm_constant( 1.0 )
{
  // Make sure the electron momentum projections are valid
  testPrecondition( electron_momentum_projections.size() > 1 );
  testPrecondition( electron_momentum_projections.front() != 0.0 );
  testPrecondition( electron_momentum_projections.front() ==
		    -electron_momentum_projections.back() );
  testPrecondition( Utility::Sort::isSortedAscending(
				       electron_momentum_projections.begin(),
				       electron_momentum_projections.end() ) );
  // Make sure the compton profile is valid
  testPrecondition( compton_profile.back() > 0.0 );
  testPrecondition( compton_profile.front() == compton_profile.back() );
  testPrecondition( compton_profile.size() ==
		    electron_momentum_projections.size() );

  // Store the profile in a tabular distribution for quick interpolation
  d_compton_profile.reset( new Utility::TabularDistribution<Utility::LogLin>(
					         electron_momentum_projections,
					         compton_profile ) );

  // Roundoff errors are common in the available Compton profile tables -
  // renormalize the table
  d_compton_profile_norm_constant = this->evaluateOccupationNumber(
			                  electron_momentum_projections.back(),
					  norm_constant_precision );
}

// Return the normalization constant used with the Compton profile
double OccupationNumberEvaluator::getComptonProfileNormConstant() const
{
  return d_compton_profile_norm_constant;
}

// Evaluate the compton profile
double OccupationNumberEvaluator::evaluateComptonProfile(
			      const double electron_momentum_projection ) const
{
  // Make sure the electron momentum projection is valid
  testPrecondition( !Teuchos::ScalarTraits<double>::isnaninf(
					      electron_momentum_projection ) );

  return d_compton_profile->evaluate( electron_momentum_projection )/
    d_compton_profile_norm_constant;
}

// Evaluate the occupation number at a given electron momentum projection
double OccupationNumberEvaluator::evaluateOccupationNumber(
				     const double electron_momentum_projection,
				     const double precision ) const
{
  // Make sure the electron momentum projection is valid
  testPrecondition( !Teuchos::ScalarTraits<double>::isnaninf(
					      electron_momentum_projection ) );

  double occupation_number;

  if( electron_momentum_projection <=
      d_compton_profile->getLowerBoundOfIndepVar() )
  {
    occupation_number = 0.0;
  }
  else if( electron_momentum_projection >
	   d_compton_profile->getLowerBoundOfIndepVar() )
  {
    boost::function<double (double pz)> compton_profile_wrapper =
      boost::bind<double>( &OccupationNumberEvaluator::evaluateComptonProfile,
			   boost::cref( *this ),
			   _1 );

    double abs_error;

    Utility::GaussKronrodIntegrator quadrature_gkq( precision );

    if( electron_momentum_projection <
	d_compton_profile->getUpperBoundOfIndepVar() )
    {
      quadrature_gkq.integrateAdaptively<15>(
				  compton_profile_wrapper,
				  d_compton_profile->getLowerBoundOfIndepVar(),
				  electron_momentum_projection,
				  occupation_number,
				  abs_error );
    }
    else
    {
      quadrature_gkq.integrateAdaptively<15>(
				  compton_profile_wrapper,
				  d_compton_profile->getLowerBoundOfIndepVar(),
				  d_compton_profile->getUpperBoundOfIndepVar(),
				  occupation_number,
				  abs_error );
    }
  }

  // Make sure the occupation number is valid
  testPostcondition( occupation_number >= 0.0 );

  return occupation_number;
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_OccupationNumberEvaluator.cpp
//---------------------------------------------------------------------------//


/*
Brian Staber (brian.staber@gmail.com)
*/

#ifndef NEUMANNINNERSURFACE_STOCHASTICPOLYCONVEXHGO_HPP
#define NEUMANNINNERSURFACE_STOCHASTICPOLYCONVEXHGO_HPP

#include "tensor_calculus.hpp"
#include "nearlyIncompressibleHyperelasticity.hpp"
#include "laplacepp.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

class neumannInnerSurface_StochasticPolyconvexHGO : public nearlyIncompressibleHyperelasticity
{
public:

    laplace * Laplace;

    double w1, w2, w3, w4;
    double mean_c1, c1, deltaC1;
    double mean_c2, c2, deltaC2;
    double mean_u1, u1, deltaU1;
    double mean_mu4, mu4, deltaG4;
    double mean_mu1, mu1;
    double mean_mu2, mu2;
    double mean_mu3, mu3;
    double alpha1, alpha2;
    double alpha3, alpha4;
    double tau1, tau2;
    double alpha5, alpha6;
    double beta3, beta4;
    double theta;
    double epsilon = 1e-6;

    Epetra_IntSerialDenseVector cells_nodes_p1_med;
    Epetra_SerialDenseVector w1_gmrf, w2_gmrf, w3_gmrf, w4_gmrf;
    Epetra_SerialDenseVector a, b;
    Epetra_SerialDenseVector E1,E2,E3;
    Epetra_SerialDenseVector N;

    neumannInnerSurface_StochasticPolyconvexHGO(Epetra_Comm & comm, Teuchos::ParameterList & Parameters){

        std::string mesh_file               = Teuchos::getParameter<std::string>(Parameters.sublist("Mesh"),  "mesh_file");
        std::string boundary_file           = Teuchos::getParameter<std::string>(Parameters.sublist("Mesh"),  "boundary_file");
        unsigned int number_physical_groups = Teuchos::getParameter<unsigned int>(Parameters.sublist("Mesh"), "nb_phys_groups");
        std::string select_model            = Teuchos::getParameter<std::string>(Parameters.sublist("Mesh"),  "model");

        mean_mu1   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu1");
        mean_mu2   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu2");
        mean_mu3   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu3");
        mean_mu4   = Teuchos::getParameter<double>(Parameters.sublist(select_model), "mu4");
        beta3      = Teuchos::getParameter<double>(Parameters.sublist(select_model), "beta3");
        beta4      = Teuchos::getParameter<double>(Parameters.sublist(select_model), "beta4");
        theta      = Teuchos::getParameter<double>(Parameters.sublist(select_model), "theta");
        deltaC1    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaC1");
        deltaC2    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaC2");
        deltaU1    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaU1");
        deltaG4    = Teuchos::getParameter<double>(Parameters.sublist(select_model), "deltaG4");

        mean_c1 = 2.0*mean_mu3*beta3*beta3;
        mean_c2 = 2.0*mean_mu1 + std::sqrt(3.0)*3.0*mean_mu2;
        mean_u1 = 2.0*mean_mu1/mean_c2;

        double gamma = 2.0*mean_mu1/(std::sqrt(3.0)*3.0*mean_mu2);
        tau2         = (1.0 - deltaU1*deltaU1)/(deltaU1*deltaU1*gamma*(gamma+1.0));
        tau1         = (2.0*mean_mu1/(std::sqrt(3.0)*3.0*mean_mu2))*tau2;

        alpha1 = 1.0/(deltaC1*deltaC1);
        alpha2 = mean_c1*deltaC1*deltaC1;
        alpha3 = 1.0/(deltaC2*deltaC2);
        alpha4 = mean_c2*deltaC2*deltaC2;
        alpha5 = 1.0/(deltaG4*deltaG4);
        alpha6 = mean_mu4*deltaG4*deltaG4;

        Mesh = new mesh(comm, Parameters); //mesh_file, 1000.0);
        Mesh->read_boundary_file(boundary_file,number_physical_groups);
        Comm = Mesh->Comm;

        Laplace = new laplace(*Mesh);

        //$$Laplace
        Epetra_FECrsMatrix matrix(Copy,*Laplace->FEGraph);
        Epetra_Vector psi(*Laplace->StandardMap);
        Epetra_Vector phi(*Laplace->StandardMap);
        Epetra_FEVector rhs(*Laplace->StandardMap);
        int bc_indx[2];
        double bc_val[2];
        bc_val[0] = 0.0; bc_val[1] = 1.0;
        //Problem number one with aztec
        bc_indx[0] = 2; bc_indx[1] = 3;
        Laplace->solve_aztec(Parameters.sublist("Laplace"), matrix, phi, rhs, &bc_indx[0], &bc_val[0]);
        Laplace->print_solution(phi, "laplace_inlet_to_outlet_aztec.mtx");
        //Problem number one with aztec
        bc_indx[0] = 0; bc_indx[1] = 1;
        Laplace->solve_aztec(Parameters.sublist("Laplace"), matrix, psi, rhs, &bc_indx[0], &bc_val[0]);
        Laplace->print_solution(psi, "laplace_inner_to_outer_aztec.mtx");
        //Get local directions
        Laplace->compute_local_directions(phi, psi);
        Laplace->compute_center_local_directions(phi, psi);
        //$$End

        StandardMap = new Epetra_Map(-1,3*Mesh->n_local_nodes_without_ghosts,&Mesh->local_dof_without_ghosts[0],0,*Comm);
        OverlapMap = new Epetra_Map(-1,3*Mesh->n_local_nodes,&Mesh->local_dof[0],0,*Comm);
        ImportToOverlapMap = new Epetra_Import(*OverlapMap,*StandardMap);
        create_FECrsGraph();

        a.Resize(3);
        b.Resize(3);
        E1.Resize(3);
        E2.Resize(3);
        E3.Resize(3);
        N.Resize(4);
        setup_dirichlet_conditions();
    }

    ~neumannInnerSurface_StochasticPolyconvexHGO(){
    }

    void get_media(unsigned int & n_cells, unsigned int & n_nodes, std::string & path){

        std::ifstream connectivity_file_med;
        connectivity_file_med.open(path);

        w1_gmrf.Resize(n_nodes);
        w2_gmrf.Resize(n_nodes);
        w3_gmrf.Resize(n_nodes);
        w4_gmrf.Resize(n_nodes);
        if (connectivity_file_med.is_open()){
            cells_nodes_p1_med.Resize(4*n_cells);
            for (unsigned int e=0; e<4*n_cells; ++e){
                connectivity_file_med >> cells_nodes_p1_med[e];
                cells_nodes_p1_med[e] = cells_nodes_p1_med[e]-1;
            }
            connectivity_file_med.close();
        }
        else{
            std::cout << "Couldn't open the connectivity file for the media.\n";
        }

    }

    void get_matrix_and_rhs(Epetra_Vector & x, Epetra_FECrsMatrix & K, Epetra_FEVector & F){
        assembleMixedDirichletDeformationDependentNeumann_homogeneousForcing(x,K,F);
    }

    void setup_dirichlet_conditions(){
        //setup_clamp();
        setup_semislipbc();
        //setup_slipbc_and_clamp();
        //setup_slipbc();
    }

    void apply_dirichlet_conditions(Epetra_FECrsMatrix & K, Epetra_FEVector & F, double & displacement){
        //apply_clamp(K,F,displacement);
        apply_semiselipbc(K,F,displacement);
        //apply_slipbc_and_clamp(K,F,displacement);
        //apply_slipbc(K,F,displacement);
    }

    void get_material_parameters(unsigned int & e_lid, unsigned int & gp){

        int n_gauss_points = Mesh->n_gauss_cells;
        int e_gid = Mesh->local_cells[e_lid];
        int node;
        double xi = Mesh->xi_cells[gp]; double eta = Mesh->eta_cells[gp]; double zeta = Mesh->zeta_cells[gp];
        tetra4::shape_functions(N,xi,eta,zeta);

        w1 = 0.0; w2 = 0.0; w3 = 0.0; w4 = 0.0;
        for (unsigned int j=0; j<4; ++j){
            node = cells_nodes_p1_med(4*e_gid+j);
            w1 += N(j)*w1_gmrf(node);
            w2 += N(j)*w2_gmrf(node);
            w3 += N(j)*w3_gmrf(node);
            w4 += N(j)*w4_gmrf(node);
        }

        c1  = icdf_gamma(w1,alpha1,alpha2);
        c2  = icdf_gamma(w2,alpha3,alpha4);
        u1  = icdf_beta (w3,tau1,tau2);
        mu4 = icdf_gamma(w4,alpha5,alpha6);

        mu1 = ( epsilon*mean_mu1 + (1.0/2.0)*c2*u1 )/( 1.0+epsilon );
        mu2 = ( epsilon*mean_mu2 + (1.0/(std::sqrt(3.0)*3.0))*c2*(1.0-u1) )/( 1.0+epsilon );
        mu3 = ( epsilon*mean_mu3 + c1/(2.0*beta3*beta3) )/( 1.0+epsilon );
        mu4 = ( epsilon*mean_mu4 + mu4 )/( 1.0+epsilon );

        for (int i=0; i<3; ++i){
            E1(i) = Laplace->laplace_direction_one(n_gauss_points*e_lid+gp,i);
            E2(i) = Laplace->laplace_direction_two_cross_one(n_gauss_points*e_lid+gp,i);
            E3(i) = Laplace->laplace_direction_two(n_gauss_points*e_lid+gp,i);
            a(i) = cos(theta)*E1(i) + sin(theta)*E2(i);
            b(i) = cos(theta)*E1(i) - sin(theta)*E2(i);
        }

    }

    double icdf_gamma(double & w, double & alpha, double & beta){
        double erfx = boost::math::erf<double>(w/std::sqrt(2.0));
        double y = (1.0/2.0)*(1.0 + erfx);
        double yinv = boost::math::gamma_p_inv<double,double>(alpha,y);
        double z = yinv*beta;
        return z;
    }

    double icdf_beta(double & w, double & tau1, double & tau2){
        double erfx = boost::math::erf<double>(w/std::sqrt(2.0));
        double y = (1.0/2.0)*(1.0 + erfx);
        double z = boost::math::ibeta_inv<double,double,double>(tau1,tau2,y);
        return z;
    }

    void get_constitutive_tensors_static_condensation(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseVector & inverse_cauchy, Epetra_SerialDenseVector & piola_isc, Epetra_SerialDenseVector & piola_vol, Epetra_SerialDenseMatrix & tangent_piola_isc, Epetra_SerialDenseMatrix & tangent_piola_vol){
        model_C(deformation_gradient, det, inverse_cauchy, piola_isc, piola_vol, tangent_piola_isc, tangent_piola_vol);
    }

    void get_internal_pressure(double & theta, double & pressure, double & dpressure){
        double ptheta = std::pow(theta,beta3);
        pressure  = beta3*( (ptheta/theta) - (1.0/(ptheta*theta)) );
        dpressure = beta3*( (beta3-1.0)*(ptheta/(theta*theta)) + (beta3+1.0)/(ptheta*theta*theta) );
    }

    void get_material_parameters_for_recover(unsigned int & e_lid){

      int e_gid = Mesh->local_cells[e_lid];
      double xi = 1.0/3.0; double eta = 1.0/3.0; double zeta = 1.0/3.0;
      tetra4::shape_functions(N,xi,eta,zeta);

      w1 = 0.0; w2 = 0.0; w3 = 0.0; w4 = 0.0;
      for (unsigned int j=0; j<4; ++j){
          int node = cells_nodes_p1_med(4*e_gid+j);
          w1 += N(j)*w1_gmrf(node);
          w2 += N(j)*w2_gmrf(node);
          w3 += N(j)*w3_gmrf(node);
          w4 += N(j)*w4_gmrf(node);
      }

      c1  = icdf_gamma(w1,alpha1,alpha2);
      c2  = icdf_gamma(w2,alpha3,alpha4);
      u1  = icdf_beta (w3,tau1,tau2);
      mu4 = icdf_gamma(w4,alpha5,alpha6);

      mu1 = ( epsilon*mean_mu1 + (1.0/2.0)*c2*u1 )/( 1.0+epsilon );
      mu2 = ( epsilon*mean_mu2 + (1.0/(std::sqrt(3.0)*3.0))*c2*(1.0-u1) )/( 1.0+epsilon );
      mu3 = ( epsilon*mean_mu3 + c1/(2.0*beta3*beta3) )/( 1.0+epsilon );
      mu4 = ( epsilon*mean_mu4 + mu4 )/( 1.0+epsilon );

      for (int i=0; i<3; ++i){
          E1(i) = Laplace->laplace_direction_one_center(e_lid,i);
          E2(i) = Laplace->laplace_direction_two_cross_one_center(e_lid,i);
          E3(i) = Laplace->laplace_direction_two_center(e_lid,i);
          a(i) = cos(theta)*E1(i) + sin(theta)*E2(i);
          b(i) = cos(theta)*E1(i) - sin(theta)*E2(i);
      }

    }

    void get_stress_for_recover(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseMatrix & piola_stress){

        det = deformation_gradient(0,0)*deformation_gradient(1,1)*deformation_gradient(2,2)
            -deformation_gradient(0,0)*deformation_gradient(1,2)*deformation_gradient(2,1)
            -deformation_gradient(0,1)*deformation_gradient(1,0)*deformation_gradient(2,2)
            +deformation_gradient(0,1)*deformation_gradient(1,2)*deformation_gradient(2,0)
            +deformation_gradient(0,2)*deformation_gradient(1,0)*deformation_gradient(2,1)
            -deformation_gradient(0,2)*deformation_gradient(1,1)*deformation_gradient(2,0);

        double alpha = std::pow(det,-2.0/3.0);
        double beta = 1.0/(det*det);
        Epetra_SerialDenseMatrix eye(3,3);
        Epetra_SerialDenseMatrix M1(3,3), M2(3,3);
        Epetra_SerialDenseMatrix C(3,3), L(3,3);
        Epetra_SerialDenseMatrix piola_ani1(3,3), piola_ani2(3,3);

        eye(0,0) = 1.0; eye(0,1) = 0.0; eye(0,2) = 0.0;
        eye(1,0) = 0.0; eye(1,1) = 1.0; eye(1,2) = 0.0;
        eye(2,0) = 0.0; eye(2,1) = 0.0; eye(2,2) = 1.0;

        M1.Multiply('N','T',1.0,a,a,0.0);
        M2.Multiply('N','T',1.0,b,b,0.0);

        C.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

        L(0,0) = (1.0/(det*det))*(C(1,1)*C(2,2)-C(1,2)*C(2,1));
        L(1,1) = (1.0/(det*det))*(C(0,0)*C(2,2)-C(0,2)*C(2,0));
        L(2,2) = (1.0/(det*det))*(C(0,0)*C(1,1)-C(0,1)*C(1,0));
        L(1,2) = (1.0/(det*det))*(C(0,2)*C(1,0)-C(0,0)*C(1,2));
        L(0,2) = (1.0/(det*det))*(C(0,1)*C(1,2)-C(0,2)*C(1,1));
        L(0,1) = (1.0/(det*det))*(C(0,2)*C(2,1)-C(0,1)*C(2,2));
        L(2,1) = L(1,2); L(2,0) = L(0,2); L(1,0) = L(0,1);

        double I1   = C(0,0) + C(1,1) + C(2,2);
        double II1  = C(0,0)*C(0,0) + C(1,1)*C(1,1) + C(2,2)*C(2,2) + 2.0*C(1,2)*C(1,2) + 2.0*C(0,2)*C(0,2) + 2.0*C(0,1)*C(0,1);
        double I2   = (1.0/2.0)*(I1*I1-II1);
        double I4_1 = C(0,0)*M1(0,0) + C(1,1)*M1(1,1) + C(2,2)*M1(2,2) + 2.0*C(0,1)*M1(0,1) + 2.0*C(0,2)*M1(0,2) + 2.0*C(1,2)*M1(1,2);
        double I4_2 = C(0,0)*M2(0,0) + C(1,1)*M2(1,1) + C(2,2)*M2(2,2) + 2.0*C(0,1)*M2(0,1) + 2.0*C(0,2)*M2(0,2) + 2.0*C(1,2)*M2(1,2);
        double pI2  = std::sqrt(I2);

        double S4_1 = (I4_1-1.0)*(I4_1-1.0);
        double S4_2 = (I4_2-1.0)*(I4_2-1.0);

        double ptheta = std::pow(det,beta3);
        double pressure = mu3*beta3*( (ptheta/det) - (1.0/(ptheta*det)) );

        for (unsigned int i=0; i<3; ++i){
            for (unsigned int j=0; j<3; ++j){
                piola_stress(i,j) = 2.0*mu1*alpha*(eye(i,j)-(1.0/3.0)*L(i,j))
                + mu2*beta*( 3.0*pI2*(I1*eye(i,j)-C(i,j)) - 2.0*I2*pI2*L(i,j) )
                + det*pressure*L(i,j);
                piola_ani1(i,j) = 4.0*mu4*(I4_1-1.0)*exp(beta4*S4_1)*M1(i,j);
                piola_ani2(i,j) = 4.0*mu4*(I4_2-1.0)*exp(beta4*S4_2)*M2(i,j);
            }
        }

        if (I4_1>1.0){
            piola_stress += piola_ani1;
        }
        if (I4_2>1.0){
            piola_stress += piola_ani2;
        }

    }

    void setup_clamp(){
        n_bc_dof = 0;
        for (unsigned int i=0; i<Mesh->n_local_nodes_without_ghosts; ++i){
            if (Mesh->nodes_to_boundaries(i,2)==1 || Mesh->nodes_to_boundaries(i,3)==1){
                n_bc_dof+=3;
            }
        }

        int indbc = 0;
        dof_on_boundary = new int [n_bc_dof];
        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            if (Mesh->nodes_to_boundaries(inode,2)==1 || Mesh->nodes_to_boundaries(inode,3)==1){
                dof_on_boundary[indbc+0] = 3*inode+0;
                dof_on_boundary[indbc+1] = 3*inode+1;
                dof_on_boundary[indbc+2] = 3*inode+2;
                indbc+=3;
            }
        }
    }

    void setup_slipbc(){
        const int nodesblk_gid0 = 480-1;
        const int nodesblk_gid1 = 538-1;
        const int nodesblk_gid2 = 577-1;

        int node;
        n_bc_dof = 0;
        for (unsigned int i=0; i<Mesh->n_local_nodes_without_ghosts; ++i){
            if (Mesh->nodes_to_boundaries(i,2)==1 || Mesh->nodes_to_boundaries(i,3)==1){
                node = Mesh->local_nodes[i];
                switch (node){
                    case nodesblk_gid0:
                        n_bc_dof+=2;
                        break;
                    case nodesblk_gid1:
                        n_bc_dof+=2;
                        break;
                    case nodesblk_gid2:
                        n_bc_dof+=2;
                        break;
                    default:
                        n_bc_dof+=1;
                        break;
                };
            }
        }

        int indbc = 0;
        dof_on_boundary = new int [n_bc_dof];
        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            if (Mesh->nodes_to_boundaries(inode,2)==1 || Mesh->nodes_to_boundaries(inode,3)==1){
                node = Mesh->local_nodes[inode];
                switch (node){
                    case nodesblk_gid0:
                        dof_on_boundary[indbc+0] = 3*inode+0;
                        dof_on_boundary[indbc+1] = 3*inode+1;
                        indbc+=2;
                        break;
                    case nodesblk_gid1:
                        dof_on_boundary[indbc+0] = 3*inode+0;
                        dof_on_boundary[indbc+1] = 3*inode+2;
                        indbc+=2;
                        break;
                    case nodesblk_gid2:
                        dof_on_boundary[indbc+0] = 3*inode+0;
                        dof_on_boundary[indbc+1] = 3*inode+2;
                        indbc+=2;
                        break;
                    default:
                        dof_on_boundary[indbc+0] = 3*inode+0;
                        indbc+=1;
                        break;
                };
            }
        }
    }

    void setup_semislipbc(){
        n_bc_dof = 0;
        for (unsigned int i=0; i<Mesh->n_local_nodes_without_ghosts; ++i){
            if (Mesh->nodes_to_boundaries(i,2)==1){
                n_bc_dof+=1;
            }
            if (Mesh->nodes_to_boundaries(i,3)==1){
                n_bc_dof+=3;
            }
        }

        int indbc = 0;
        dof_on_boundary = new int [n_bc_dof];
        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            if (Mesh->nodes_to_boundaries(inode,2)==1){
                dof_on_boundary[indbc] = 3*inode+0;
                indbc+=1;
            }
            if (Mesh->nodes_to_boundaries(inode,3)==1){
                dof_on_boundary[indbc+0] = 3*inode+0;
                dof_on_boundary[indbc+1] = 3*inode+1;
                dof_on_boundary[indbc+2] = 3*inode+2;
                indbc+=3;
            }
        }
    }

    void setup_slipbc_and_clamp(){
        Epetra_IntSerialDenseVector nodesblk_gid(3);
        nodesblk_gid(0) = 480-1;
        nodesblk_gid(1) = 481-1;
        nodesblk_gid(2) = 479-1;

        /*nodesblk_gid(0) = 203-1;
         nodesblk_gid(1) = 204-1;
         nodesblk_gid(2) = 205-1;
         nodesblk_gid(3) = 206-1;
         nodesblk_gid(4) = 207-1;
         nodesblk_gid(5) = 208-1;
         nodesblk_gid(6) = 281-1;
         nodesblk_gid(7) = 282-1;
         nodesblk_gid(8) = 283-1;
         nodesblk_gid(9) = 284-1;
         nodesblk_gid(10) = 285-1;
         nodesblk_gid(11) = 286-1;*/

        int node;
        n_bc_dof = 0;
        for (unsigned int i=0; i<Mesh->n_local_nodes_without_ghosts; ++i){
            if (Mesh->nodes_to_boundaries(i,2)==1 || Mesh->nodes_to_boundaries(i,3)==1){
                node = Mesh->local_nodes[i];
                if (node==nodesblk_gid(0)||node==nodesblk_gid(1)||node==nodesblk_gid(2)){
                    n_bc_dof+=3;
                }
                else{
                    n_bc_dof+=1;
                }
            }
        }

        int indbc = 0;
        dof_on_boundary = new int [n_bc_dof];
        for (unsigned int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
            if (Mesh->nodes_to_boundaries(inode,2)==1 || Mesh->nodes_to_boundaries(inode,3)==1){
                node = Mesh->local_nodes[inode];
                if (node==nodesblk_gid(0)||node==nodesblk_gid(1)||node==nodesblk_gid(2)){
                    dof_on_boundary[indbc+0] = 3*inode+0;
                    dof_on_boundary[indbc+1] = 3*inode+1;
                    dof_on_boundary[indbc+2] = 3*inode+2;
                    indbc+=3;
                }
                else{
                    dof_on_boundary[indbc+0] = 3*inode+0;
                    indbc+=1;
                }
            }
        }
    }

    void apply_clamp(Epetra_FECrsMatrix & K, Epetra_FEVector & F, double & displacement){
        if (n_bc_dof>0){
            int node;
            for (int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
                node = Mesh->local_nodes[inode];
                if (Mesh->nodes_to_boundaries(inode,2)==1 || Mesh->nodes_to_boundaries(inode,3)==1){
                    F[0][StandardMap->LID(3*node+0)] = displacement;
                    F[0][StandardMap->LID(3*node+1)] = displacement;
                    F[0][StandardMap->LID(3*node+2)] = displacement;
                }
            }
        }
        ML_Epetra::Apply_OAZToMatrix(dof_on_boundary,n_bc_dof,K);
    }

    void apply_slipbc(Epetra_FECrsMatrix & K, Epetra_FEVector & F, double & displacement){
        const int nodesblk_gid0 = 480-1;
        const int nodesblk_gid1 = 538-1;
        const int nodesblk_gid2 = 577-1;

        if (n_bc_dof>0){
            int node;
            for (int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
                node = Mesh->local_nodes[inode];
                if (Mesh->nodes_to_boundaries(inode,2)==1 || Mesh->nodes_to_boundaries(inode,3)==1){
                    switch (node){
                        case nodesblk_gid0:
                            F[0][StandardMap->LID(3*node+0)] = displacement;
                            F[0][StandardMap->LID(3*node+1)] = displacement;
                            break;
                        case nodesblk_gid1:
                            F[0][StandardMap->LID(3*node+0)] = displacement;
                            F[0][StandardMap->LID(3*node+2)] = displacement;
                            break;
                        case nodesblk_gid2:
                            F[0][StandardMap->LID(3*node+0)] = displacement;
                            F[0][StandardMap->LID(3*node+2)] = displacement;
                            break;
                        default:
                            F[0][StandardMap->LID(3*node+0)] = displacement;
                            break;
                    };
                }
            }
        }
        ML_Epetra::Apply_OAZToMatrix(dof_on_boundary,n_bc_dof,K);
    }

    void apply_semiselipbc(Epetra_FECrsMatrix & K, Epetra_FEVector & F, double & displacement){
        if (n_bc_dof>0){
            int node;
            for (int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
                node = Mesh->local_nodes[inode];
                if (Mesh->nodes_to_boundaries(inode,2)==1){
                    F[0][StandardMap->LID(3*node+0)] = displacement;
                }
                if (Mesh->nodes_to_boundaries(inode,3)==1){
                    F[0][StandardMap->LID(3*node+0)] = displacement;
                    F[0][StandardMap->LID(3*node+1)] = displacement;
                    F[0][StandardMap->LID(3*node+2)] = displacement;
                }
            }
        }
        ML_Epetra::Apply_OAZToMatrix(dof_on_boundary,n_bc_dof,K);
    }

    void apply_slipbc_and_clamp(Epetra_FECrsMatrix & K, Epetra_FEVector & F, double & displacement){
        Epetra_IntSerialDenseVector nodesblk_gid(3);
        nodesblk_gid(0) = 480-1;
        nodesblk_gid(1) = 481-1;
        nodesblk_gid(2) = 479-1;

        if (n_bc_dof>0){
            int node;
            for (int inode=0; inode<Mesh->n_local_nodes_without_ghosts; ++inode){
                node = Mesh->local_nodes[inode];
                if (Mesh->nodes_to_boundaries(inode,2)==1 || Mesh->nodes_to_boundaries(inode,3)==1){
                    if (node==nodesblk_gid(0)||node==nodesblk_gid(1)||node==nodesblk_gid(2)){
                        F[0][StandardMap->LID(3*node+0)] = displacement;
                        F[0][StandardMap->LID(3*node+1)] = displacement;
                        F[0][StandardMap->LID(3*node+2)] = displacement;
                    }
                    else{
                        F[0][StandardMap->LID(3*node+0)] = displacement;
                    }
                }
            }
        }
        ML_Epetra::Apply_OAZToMatrix(dof_on_boundary,n_bc_dof,K);
    }

    void model_C(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseVector & L, Epetra_SerialDenseVector & piola_isc, Epetra_SerialDenseVector & piola_vol, Epetra_SerialDenseMatrix & tangent_piola_isc, Epetra_SerialDenseMatrix & tangent_piola_vol){

        det = deformation_gradient(0,0)*deformation_gradient(1,1)*deformation_gradient(2,2)
            -deformation_gradient(0,0)*deformation_gradient(1,2)*deformation_gradient(2,1)
            -deformation_gradient(0,1)*deformation_gradient(1,0)*deformation_gradient(2,2)
            +deformation_gradient(0,1)*deformation_gradient(1,2)*deformation_gradient(2,0)
            +deformation_gradient(0,2)*deformation_gradient(1,0)*deformation_gradient(2,1)
            -deformation_gradient(0,2)*deformation_gradient(1,1)*deformation_gradient(2,0);

        double alpha = std::pow(det,-2.0/3.0);

        Epetra_SerialDenseVector eye(6);
        Epetra_SerialDenseMatrix rightCauchy(3,3);
        Epetra_SerialDenseVector M1(6), M2(6);
        Epetra_SerialDenseVector C(6), D(6);
        Epetra_SerialDenseVector piola_nh(6), piola_ani1(6), piola_ani2(6);

        M1(0) = a(0)*a(0); M2(0) = b(0)*b(0);
        M1(1) = a(1)*a(1); M2(1) = b(1)*b(1);
        M1(2) = a(2)*a(2); M2(2) = b(2)*b(2);
        M1(3) = a(1)*a(2); M2(3) = b(1)*b(2);
        M1(4) = a(0)*a(2); M2(4) = b(0)*b(2);
        M1(5) = a(0)*a(1); M2(5) = b(0)*b(1);

        rightCauchy.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

        eye(0) = 1.0; eye(1) = 1.0; eye(2) = 1.0; eye(3) = 0.0; eye(4) = 0.0; eye(5) = 0.0;

        C(0) = rightCauchy(0,0); C(1) = rightCauchy(1,1); C(2) = rightCauchy(2,2);
        C(3) = rightCauchy(1,2); C(4) = rightCauchy(0,2); C(5) = rightCauchy(0,1);

        L(0) = (1.0/(det*det))*(rightCauchy(1,1)*rightCauchy(2,2)-rightCauchy(1,2)*rightCauchy(2,1));
        L(1) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(2,2)-rightCauchy(0,2)*rightCauchy(2,0));
        L(2) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(1,1)-rightCauchy(0,1)*rightCauchy(1,0));
        L(3) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(1,0)-rightCauchy(0,0)*rightCauchy(1,2));
        L(4) = (1.0/(det*det))*(rightCauchy(0,1)*rightCauchy(1,2)-rightCauchy(0,2)*rightCauchy(1,1));
        L(5) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(2,1)-rightCauchy(0,1)*rightCauchy(2,2));

        double I1   = C(0) + C(1) + C(2);
        double II1  = C(0)*C(0) + C(1)*C(1) + C(2)*C(2) + 2.0*C(3)*C(3) + 2.0*C(4)*C(4) + 2.0*C(5)*C(5);
        double I2   = (1.0/2.0)*(I1*I1-II1);
        double I4_1 = C(0)*M1(0) + C(1)*M1(1) + C(2)*M1(2) + 2.0*C(5)*M1(5) + 2.0*C(4)*M1(4) + 2.0*C(3)*M1(3);
        double I4_2 = C(0)*M2(0) + C(1)*M2(1) + C(2)*M2(2) + 2.0*C(5)*M2(5) + 2.0*C(4)*M2(4) + 2.0*C(3)*M2(3);
        double pI2  = std::sqrt(I2);

        double S4_1 = (I4_1-1.0)*(I4_1-1.0);
        double S4_2 = (I4_2-1.0)*(I4_2-1.0);

        for (unsigned int i=0; i<6; ++i){
            D(i) = I1*eye(i) - C(i);
            piola_nh(i) = 2.0*alpha*mu1*(eye(i)-(1.0/3.0)*I1*L(i));
            piola_isc(i) = piola_nh(i) + (mu2/(det*det))*( 3.0*pI2*D(i) - 2.0*pI2*I2*L(i) );

            piola_ani1(i) = 4.0*mu4*(I4_1-1.0)*exp(beta4*S4_1)*M1(i);
            piola_ani2(i) = 4.0*mu4*(I4_2-1.0)*exp(beta4*S4_2)*M2(i);

            piola_vol(i) = mu3*det*L(i);
        }

        double scalarAB;

        scalarAB = mu3*det;
        tensor_product(mu3*det,L,L,tangent_piola_vol,0.0);
        scalarAB = -2.0*mu3*det;
        sym_tensor_product(scalarAB,L,L,tangent_piola_vol,1.0);

        scalarAB = -2.0/3.0;
        tensor_product(scalarAB,piola_nh,L,tangent_piola_isc,0.0);
        tensor_product(scalarAB,L,piola_nh,tangent_piola_isc,1.0);

        scalarAB = -6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,D,eye,tangent_piola_isc,1.0);
        tensor_product(scalarAB,eye,D,tangent_piola_isc,1.0);

        scalarAB = (-4.0/9.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = (4.0/3.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        sym_tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = 3.0*mu2/(det*det*pI2);
        tensor_product(scalarAB,D,D,tangent_piola_isc,1.0);

        scalarAB = 6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        scalarAB = -scalarAB;
        sym_tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        if (I4_1>1.0){
            piola_isc += piola_ani1;
            scalarAB = (8.0*mu4 + 16.0*mu4*beta4*S4_1)*exp(beta4*S4_1);
            tensor_product(scalarAB,M1,M1,tangent_piola_isc,1.0);
        }
        if (I4_2>1.0){
            piola_isc += piola_ani2;
            scalarAB = (8.0*mu4 + 16.0*mu4*beta4*S4_2)*exp(beta4*S4_2);
            tensor_product(scalarAB,M2,M2,tangent_piola_isc,1.0);
        }
    }

    void model_B(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseVector & piola_isc, Epetra_SerialDenseVector & piola_vol, Epetra_SerialDenseMatrix & tangent_piola_isc, Epetra_SerialDenseMatrix & tangent_piola_vol){

        det = deformation_gradient(0,0)*deformation_gradient(1,1)*deformation_gradient(2,2)-deformation_gradient(0,0)*deformation_gradient(1,2)*deformation_gradient(2,1)-deformation_gradient(0,1)*deformation_gradient(1,0)*deformation_gradient(2,2)+deformation_gradient(0,1)*deformation_gradient(1,2)*deformation_gradient(2,0)+deformation_gradient(0,2)*deformation_gradient(1,0)*deformation_gradient(2,1)-deformation_gradient(0,2)*deformation_gradient(1,1)*deformation_gradient(2,0);

        double alpha = std::pow(det,-2.0/3.0);

        Epetra_SerialDenseMatrix rightCauchy(3,3);
        Epetra_SerialDenseVector M1(6), M2(6);
        Epetra_SerialDenseVector eye(6);
        Epetra_SerialDenseVector C(6), D(6), L(6);
        Epetra_SerialDenseVector dI4_1(6), dI4_2(6);
        Epetra_SerialDenseVector piola_nh(6), piola_ani1(6), piola_ani2(6);

        M1(0) = a(0)*a(0); M2(0) = b(0)*b(0);
        M1(1) = a(1)*a(1); M2(1) = b(1)*b(1);
        M1(2) = a(2)*a(2); M2(2) = b(2)*b(2);
        M1(3) = a(1)*a(2); M2(3) = b(1)*b(2);
        M1(4) = a(0)*a(2); M2(4) = b(0)*b(2);
        M1(5) = a(0)*a(1); M2(5) = b(0)*b(1);

        rightCauchy.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

        eye(0) = 1.0; eye(1) = 1.0; eye(2) = 1.0; eye(3) = 0.0; eye(4) = 0.0; eye(5) = 0.0;

        C(0) = rightCauchy(0,0); C(1) = rightCauchy(1,1); C(2) = rightCauchy(2,2);
        C(3) = rightCauchy(1,2); C(4) = rightCauchy(0,2); C(5) = rightCauchy(0,1);

        L(0) = (1.0/(det*det))*(rightCauchy(1,1)*rightCauchy(2,2)-rightCauchy(1,2)*rightCauchy(2,1));
        L(1) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(2,2)-rightCauchy(0,2)*rightCauchy(2,0));
        L(2) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(1,1)-rightCauchy(0,1)*rightCauchy(1,0));
        L(3) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(1,0)-rightCauchy(0,0)*rightCauchy(1,2));
        L(4) = (1.0/(det*det))*(rightCauchy(0,1)*rightCauchy(1,2)-rightCauchy(0,2)*rightCauchy(1,1));
        L(5) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(2,1)-rightCauchy(0,1)*rightCauchy(2,2));

        double I1 = C(0) + C(1) + C(2);
        double II1 = C(0)*C(0) + C(1)*C(1) + C(2)*C(2) + 2.0*C(3)*C(3) + 2.0*C(4)*C(4) + 2.0*C(5)*C(5);
        double I2 = (1.0/2.0)*(I1*I1-II1);
        double I4_1 = C(0)*M1(0) + C(1)*M1(1) + C(2)*M1(2) + 2.0*C(5)*M1(5) + 2.0*C(4)*M1(4) + 2.0*C(3)*M1(3);
        double I4_2 = C(0)*M2(0) + C(1)*M2(1) + C(2)*M2(2) + 2.0*C(5)*M2(5) + 2.0*C(4)*M2(4) + 2.0*C(3)*M2(3);
        double pI2 = std::sqrt(I2);

        double S4_1 = (alpha*I4_1-1.0)*(alpha*I4_1-1.0);
        double S4_2 = (alpha*I4_2-1.0)*(alpha*I4_2-1.0);

        for (unsigned int i=0; i<6; ++i){
            D(i) = I1*eye(i) - C(i);
            dI4_1(i) = M1(i)-(1.0/3.0)*I4_1*L(i);
            dI4_2(i) = M2(i)-(1.0/3.0)*I4_2*L(i);
            piola_nh(i) = 2.0*alpha*mu1*(eye(i)-(1.0/3.0)*I1*L(i));
            piola_isc(i) = piola_nh(i) + (mu2/(det*det))*( 3.0*pI2*D(i) - 2.0*pI2*I2*L(i) );

            piola_ani1(i) = 4.0*mu4*alpha*(alpha*I4_1-1.0)*exp(beta4*S4_1)*(M1(i)-(1.0/3.0)*I4_1*L(i));
            piola_ani2(i) = 4.0*mu4*alpha*(alpha*I4_2-1.0)*exp(beta4*S4_2)*(M2(i)-(1.0/3.0)*I4_2*L(i));;

            piola_vol(i) = det*L(i);
        }

        double scalarAB;

        scalarAB = det;
        tensor_product(det,L,L,tangent_piola_vol,0.0);
        scalarAB = -2.0*det;
        sym_tensor_product(scalarAB,L,L,tangent_piola_vol,1.0);

        scalarAB = -2.0/3.0;
        tensor_product(scalarAB,piola_nh,L,tangent_piola_isc,0.0);
        tensor_product(scalarAB,L,piola_nh,tangent_piola_isc,1.0);

        scalarAB = -6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,D,eye,tangent_piola_isc,1.0);
        tensor_product(scalarAB,eye,D,tangent_piola_isc,1.0);

        scalarAB = (-4.0/9.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = (4.0/3.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        sym_tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = 3.0*mu2/(det*det*pI2);
        tensor_product(scalarAB,D,D,tangent_piola_isc,1.0);

        scalarAB = 6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        scalarAB = -scalarAB;
        sym_tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        if (I4_1>1.0){
            piola_isc += piola_ani1;
            scalarAB = -(8.0/3.0)*mu4*alpha*(alpha*I4_1-1.0)*exp(beta4*S4_1);
            tensor_product(scalarAB,dI4_1,L,tangent_piola_isc,1.0);
            scalarAB = (8.0/3.0)*mu4*alpha*alpha*exp(beta4*S4_1);
            tensor_product(scalarAB,dI4_1,dI4_1,tangent_piola_isc,1.0);
            scalarAB = 16.0*mu4*beta4*alpha*alpha*S4_1*exp(beta4*S4_1);
            tensor_product(scalarAB,dI4_1,dI4_1,tangent_piola_isc,1.0);
        }
        if (I4_2>1.0){
            piola_isc += piola_ani2;
            scalarAB = -(8.0/3.0)*mu4*alpha*(alpha*I4_2-1.0)*exp(beta4*S4_2);
            tensor_product(scalarAB,dI4_2,L,tangent_piola_isc,1.0);
            scalarAB = (8.0/3.0)*mu4*alpha*alpha*exp(beta4*S4_2);
            tensor_product(scalarAB,dI4_2,dI4_2,tangent_piola_isc,1.0);
            scalarAB = 16.0*mu4*beta4*alpha*alpha*S4_2*exp(beta4*S4_2);
            tensor_product(scalarAB,dI4_2,dI4_2,tangent_piola_isc,1.0);
        }

    }

    void model_A(Epetra_SerialDenseMatrix & deformation_gradient, double & det, Epetra_SerialDenseVector & piola_isc, Epetra_SerialDenseVector & piola_vol, Epetra_SerialDenseMatrix & tangent_piola_isc, Epetra_SerialDenseMatrix & tangent_piola_vol){

        det = deformation_gradient(0,0)*deformation_gradient(1,1)*deformation_gradient(2,2)-deformation_gradient(0,0)*deformation_gradient(1,2)*deformation_gradient(2,1)-deformation_gradient(0,1)*deformation_gradient(1,0)*deformation_gradient(2,2)+deformation_gradient(0,1)*deformation_gradient(1,2)*deformation_gradient(2,0)+deformation_gradient(0,2)*deformation_gradient(1,0)*deformation_gradient(2,1)-deformation_gradient(0,2)*deformation_gradient(1,1)*deformation_gradient(2,0);

        double alpha = std::pow(det,-2.0/3.0);

        Epetra_SerialDenseMatrix rightCauchy(3,3);
        Epetra_SerialDenseVector M1(6), M2(6);
        Epetra_SerialDenseVector eye(6);
        Epetra_SerialDenseVector C(6), L(6), CC(6);
        Epetra_SerialDenseVector CMMC1(6), CMMC2(6);
        Epetra_SerialDenseVector D(6);
        Epetra_SerialDenseVector dK3_1(6), dK3_2(6);
        Epetra_SerialDenseVector piola_nh(6), piola_ani1(6), piola_ani2(6);

        M1(0) = a(0)*a(0);
        M1(1) = a(1)*a(1);
        M1(2) = a(2)*a(2);
        M1(3) = a(1)*a(2);
        M1(4) = a(0)*a(2);
        M1(5) = a(0)*a(1);

        M2(0) = b(0)*b(0);
        M2(1) = b(1)*b(1);
        M2(2) = b(2)*b(2);
        M2(3) = b(1)*b(2);
        M2(4) = b(0)*b(2);
        M2(5) = b(0)*b(1);

        rightCauchy.Multiply('T','N',1.0,deformation_gradient,deformation_gradient,0.0);

        eye(0) = 1.0; eye(1) = 1.0; eye(2) = 1.0; eye(3) = 0.0; eye(4) = 0.0; eye(5) = 0.0;

        C(0) = rightCauchy(0,0); C(1) = rightCauchy(1,1); C(2) = rightCauchy(2,2);
        C(3) = rightCauchy(1,2); C(4) = rightCauchy(0,2); C(5) = rightCauchy(0,1);

        L(0) = (1.0/(det*det))*(rightCauchy(1,1)*rightCauchy(2,2)-rightCauchy(1,2)*rightCauchy(2,1));
        L(1) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(2,2)-rightCauchy(0,2)*rightCauchy(2,0));
        L(2) = (1.0/(det*det))*(rightCauchy(0,0)*rightCauchy(1,1)-rightCauchy(0,1)*rightCauchy(1,0));
        L(3) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(1,0)-rightCauchy(0,0)*rightCauchy(1,2));
        L(4) = (1.0/(det*det))*(rightCauchy(0,1)*rightCauchy(1,2)-rightCauchy(0,2)*rightCauchy(1,1));
        L(5) = (1.0/(det*det))*(rightCauchy(0,2)*rightCauchy(2,1)-rightCauchy(0,1)*rightCauchy(2,2));

        CMMC1(0) = 2.0*C(0)*M1(0) + 2.0*C(4)*M1(4) + 2.0*C(5)*M1(5);
        CMMC1(1) = 2.0*C(1)*M1(1) + 2.0*C(3)*M1(3) + 2.0*C(5)*M1(5);
        CMMC1(2) = 2.0*C(2)*M1(2) + 2.0*C(3)*M1(3) + 2.0*C(4)*M1(4);
        CMMC1(3) = C(1)*M1(3) + C(3)*M1(1) + C(2)*M1(3) + C(3)*M1(2) + C(4)*M1(5) + C(5)*M1(4);
        CMMC1(4) = C(0)*M1(4) + C(4)*M1(0) + C(2)*M1(4) + C(4)*M1(2) + C(3)*M1(5) + C(5)*M1(3);
        CMMC1(5) = C(0)*M1(5) + C(5)*M1(0) + C(1)*M1(5) + C(5)*M1(1) + C(3)*M1(4) + C(4)*M1(3);

        CMMC2(0) = 2.0*C(0)*M2(0) + 2.0*C(4)*M2(4) + 2.0*C(5)*M2(5);
        CMMC2(1) = 2.0*C(1)*M2(1) + 2.0*C(3)*M2(3) + 2.0*C(5)*M2(5);
        CMMC2(2) = 2.0*C(2)*M2(2) + 2.0*C(3)*M2(3) + 2.0*C(4)*M2(4);
        CMMC2(3) = C(1)*M2(3) + C(3)*M2(1) + C(2)*M2(3) + C(3)*M2(2) + C(4)*M2(5) + C(5)*M2(4);
        CMMC2(4) = C(0)*M2(4) + C(4)*M2(0) + C(2)*M2(4) + C(4)*M2(2) + C(3)*M2(5) + C(5)*M2(3);
        CMMC2(5) = C(0)*M2(5) + C(5)*M2(0) + C(1)*M2(5) + C(5)*M2(1) + C(3)*M2(4) + C(4)*M2(3);

        CC(0) = C(0)*C(0) + C(4)*C(4) + C(5)*C(5);
        CC(1) = C(1)*C(1) + C(3)*C(3) + C(5)*C(5);
        CC(2) = C(2)*C(2) + C(3)*C(3) + C(4)*C(4);
        CC(3) = C(1)*C(3) + C(2)*C(3) + C(4)*C(5);
        CC(4) = C(0)*C(4) + C(2)*C(4) + C(3)*C(5);
        CC(5) = C(0)*C(5) + C(1)*C(5) + C(3)*C(4);

        double I1 = C(0) + C(1) + C(2);
        double II1 = C(0)*C(0) + C(1)*C(1) + C(2)*C(2) + 2.0*C(3)*C(3) + 2.0*C(4)*C(4) + 2.0*C(5)*C(5);
        double I2 = (1.0/2.0)*(I1*I1-II1);
        double I4_1 = C(0)*M1(0) + C(1)*M1(1) + C(2)*M1(2) + 2.0*C(5)*M1(5) + 2.0*C(4)*M1(4) + 2.0*C(3)*M1(3);
        double I4_2 = C(0)*M2(0) + C(1)*M2(1) + C(2)*M2(2) + 2.0*C(5)*M2(5) + 2.0*C(4)*M2(4) + 2.0*C(3)*M2(3);
        double I5_1 = CC(0)*M1(0) + CC(1)*M1(1) + CC(2)*M1(2) + 2.0*CC(5)*M1(5) + 2.0*CC(4)*M1(4) + 2.0*CC(3)*M1(3);
        double I5_2 = CC(0)*M2(0) + CC(1)*M2(1) + CC(2)*M2(2) + 2.0*CC(5)*M2(5) + 2.0*CC(4)*M2(4) + 2.0*CC(3)*M2(3);
        double K3_1 = I1*I4_1-I5_1;
        double K3_2 = I1*I4_2-I5_2;
        double pI2 = std::sqrt(I2);

        for (unsigned int i=0; i<6; ++i){
            D(i) = I1*eye(i) - C(i);
            piola_nh(i) = 2.0*alpha*mu1*(eye(i)-(1.0/3.0)*I1*L(i));
            piola_isc(i) = piola_nh(i) + (mu2/(det*det))*( 3.0*pI2*D(i) - 2.0*pI2*I2*L(i) );

            dK3_1(i) = I4_1*eye(i) + I1*M1(i) - CMMC1(i);
            dK3_2(i) = I4_2*eye(i) + I1*M2(i) - CMMC2(i);
            piola_ani1(i) = 2.0*mu4*beta4*std::pow(K3_1-2.0,beta4-1.0)*dK3_1(i);
            piola_ani2(i) = 2.0*mu4*beta4*std::pow(K3_2-2.0,beta4-1.0)*dK3_2(i);

            piola_vol(i) = det*L(i);
        }

        double scalarAB;

        scalarAB = det;
        tensor_product(det,L,L,tangent_piola_vol,0.0);
        scalarAB = -2.0*det;
        sym_tensor_product(scalarAB,L,L,tangent_piola_vol,1.0);

        scalarAB = -2.0/3.0;
        tensor_product(scalarAB,piola_nh,L,tangent_piola_isc,0.0);
        tensor_product(scalarAB,L,piola_nh,tangent_piola_isc,1.0);

        scalarAB = -6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,D,eye,tangent_piola_isc,1.0);
        tensor_product(scalarAB,eye,D,tangent_piola_isc,1.0);

        scalarAB = (-4.0/9.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = (4.0/3.0)*mu1*alpha*I1 + 4.0*mu2*pI2*I2/(det*det);
        sym_tensor_product(scalarAB,L,L,tangent_piola_isc,1.0);

        scalarAB = 3.0*mu2/(det*det*pI2);
        tensor_product(scalarAB,D,D,tangent_piola_isc,1.0);

        scalarAB = 6.0*mu2*pI2/(det*det);
        tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        scalarAB = -scalarAB;
        sym_tensor_product(scalarAB,eye,eye,tangent_piola_isc,1.0);

        if (K3_1>2.0){
            piola_isc += piola_ani1;
            scalarAB = 4.0*mu4*beta4*(beta4-1.0)*std::pow(K3_1-2.0,beta4-2.0);
            tensor_product(scalarAB,dK3_1,dK3_1,tangent_piola_isc,1.0);

            scalarAB = 4.0*mu4*beta4*std::pow(K3_1-2.0,beta4-1.0);
            tensor_product(scalarAB,eye,M1,tangent_piola_isc,1.0);
            tensor_product(scalarAB,M1,eye,tangent_piola_isc,1.0);
            scalarAB = -scalarAB;
            sym_tensor_product(scalarAB,M1,eye,tangent_piola_isc,1.0);
            sym_tensor_product(scalarAB,eye,M1,tangent_piola_isc,1.0);
        }
        if (K3_2>2.0){
            piola_isc += piola_ani2;
            scalarAB = 4.0*mu4*beta4*(beta4-1.0)*std::pow(K3_2-2.0,beta4-2.0);
            tensor_product(scalarAB,dK3_2,dK3_2,tangent_piola_isc,1.0);

            scalarAB = 4.0*mu4*beta4*std::pow(K3_2-2.0,beta4-1.0);
            tensor_product(scalarAB,eye,M2,tangent_piola_isc,1.0);
            tensor_product(scalarAB,M2,eye,tangent_piola_isc,1.0);
            scalarAB = -scalarAB;
            sym_tensor_product(scalarAB,M2,eye,tangent_piola_isc,1.0);
            sym_tensor_product(scalarAB,eye,M2,tangent_piola_isc,1.0);
        }
    }

};

#endif


#include "registration_helpers.hpp"

#include <boost/test/unit_test.hpp>

#include "registration_check_common.hpp"

#include "defines.hpp"

using namespace database_fixture;

namespace registration_helpers {

uint64_t fuzzer::get_sin_f(uint64_t pos, uint64_t max_pos, uint64_t max_result)
{
    BOOST_REQUIRE(max_pos > 0);

    // calculate upper part of the sin-curve

    const float fpi = 3.141592653589793f;
    float fpos = pos * fpi;
    fpos /= max_pos;

    float ret = std::sin(fpos);
    ret *= max_result;

    // roound to unsigned int type

    if (pos <= max_pos / 2)
    {
        ret += 0.5f;
    }
    else
    {
        ret -= 0.5f;
    }

    if (ret < 0.f)
    {
        ret = 0.f;
    }
    return (uint64_t)floor(ret);
}

BOOST_FIXTURE_TEST_SUITE(fuzzer_check, fuzzer)

SCORUM_TEST_CASE(sin_f_check)
{
    BOOST_CHECK_EQUAL(get_sin_f(0, 10, 5), (uint64_t)0);
    BOOST_CHECK_EQUAL(get_sin_f(1, 10, 5), (uint64_t)2);
    BOOST_CHECK_EQUAL(get_sin_f(2, 10, 5), (uint64_t)3);
    BOOST_CHECK_EQUAL(get_sin_f(3, 10, 5), (uint64_t)4);
    BOOST_CHECK_EQUAL(get_sin_f(4, 10, 5), (uint64_t)5);
    BOOST_CHECK_EQUAL(get_sin_f(5, 10, 5), (uint64_t)5);
    BOOST_CHECK_EQUAL(get_sin_f(6, 10, 5), (uint64_t)4);
    BOOST_CHECK_EQUAL(get_sin_f(7, 10, 5), (uint64_t)3);
    BOOST_CHECK_EQUAL(get_sin_f(8, 10, 5), (uint64_t)2);
    BOOST_CHECK_EQUAL(get_sin_f(9, 10, 5), (uint64_t)1);

    BOOST_CHECK_EQUAL(get_sin_f(0, 5, 5), (uint64_t)0);
    BOOST_CHECK_EQUAL(get_sin_f(1, 5, 5), (uint64_t)3);
    BOOST_CHECK_EQUAL(get_sin_f(2, 5, 5), (uint64_t)5);
    BOOST_CHECK_EQUAL(get_sin_f(3, 5, 5), (uint64_t)4);
    BOOST_CHECK_EQUAL(get_sin_f(4, 5, 5), (uint64_t)2);

    BOOST_CHECK_EQUAL(get_sin_f(0, 4, 3), (uint64_t)0);
    BOOST_CHECK_EQUAL(get_sin_f(1, 4, 3), (uint64_t)2);
    BOOST_CHECK_EQUAL(get_sin_f(2, 4, 3), (uint64_t)3);
    BOOST_CHECK_EQUAL(get_sin_f(3, 4, 3), (uint64_t)1);
}

BOOST_AUTO_TEST_SUITE_END()

void predictor::initialize(const asset& registration_supply,
                           const asset& registration_bonus,
                           const schedule_inputs_type& registration_schedule)
{
    _registration_supply = registration_supply;
    _registration_bonus = registration_bonus;
    _registration_schedule = registration_schedule;
}

uint64_t predictor::schedule_input_max_pos()
{
    uint64_t ret = 0;
    for (const auto& item : get_registration_schedule())
    {
        ret += item.users;
    }
    return ret;
}

asset predictor::schedule_input_total_bonus(const asset& maximum_bonus)
{
    return database_fixture::schedule_input_total_bonus(get_registration_schedule(), maximum_bonus);
}

int predictor::schedule_input_bonus_percent_by_pos(uint64_t pos)
{
    uint64_t rest = pos;
    auto it = get_registration_schedule().begin();
    for (uint64_t stage = 0; it != get_registration_schedule().end(); ++it, ++stage)
    {
        uint64_t item_users_limit = (*it).users;

        if (rest >= item_users_limit)
        {
            rest -= item_users_limit;
        }
        else
        {
            break;
        }
    }

    if (it != get_registration_schedule().end())
    {
        return (*it).bonus_percent;
    }
    else
    {
        return 0;
    }
}

asset predictor::schedule_input_predicted_allocate_by_pos(uint64_t pos, asset maximum_bonus)
{
    share_type ret = (share_type)schedule_input_bonus_percent_by_pos(pos);
    ret *= maximum_bonus.amount;
    ret /= 100;
    return asset(ret, maximum_bonus.symbol());
}

asset predictor::schedule_input_predicted_limit(uint64_t pass_through_blocks)
{
    asset ret = get_registration_bonus();
    ret *= pass_through_blocks;
    ret *= SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_PER_N_BLOCK;
    ret /= SCORUM_REGISTRATION_BONUS_LIMIT_PER_MEMBER_N_BLOCK;
    return ret;
}

bool predictor::schedule_input_pos_reach_limit(uint64_t& pos,
                                               uint64_t max_pos,
                                               asset predicted_limit,
                                               asset maximum_bonus,
                                               std::function<asset(uint64_t pos)> allocate_cash)
{
    asset allocated(0, SCORUM_SYMBOL);
    while (pos < max_pos)
    {
        asset predicted_bonus = schedule_input_predicted_allocate_by_pos(pos, maximum_bonus);

        if (predicted_bonus.amount == share_type(0))
        {
            break;
        }

        if (allocated + predicted_bonus > predicted_limit)
        {
            // limit is reached
            return true;
        }

        allocated += allocate_cash(pos++);
    }
    return false;
}

asset predictor::schedule_input_vest_all(std::function<asset()> allocate_cash)
{
    asset total_allocated_bonus(0, SCORUM_SYMBOL);
    auto it = get_registration_schedule().begin();
    for (uint64_t input_pos = 0, stage = 0; it != get_registration_schedule().end(); ++it, ++stage)
    {
        for (uint64_t register_attempts = 0; register_attempts < (uint64_t)(*it).users;
             ++register_attempts, ++input_pos)
        {
            total_allocated_bonus += allocate_cash();
        }
    }
    return total_allocated_bonus;
}

struct predictor_fixture : public database_fixture::registration_check_fixture, public predictor
{
    predictor_fixture()
    {
        database_fixture::schedule_inputs_type schedule_input = create_registration_genesis().registration_schedule;
        initialize(registration_supply(), registration_bonus(), schedule_input);
    }
};

BOOST_FIXTURE_TEST_SUITE(predictor_check, predictor_fixture)

SCORUM_TEST_CASE(schedule_input_bonus_percent_by_pos_check)
{
    size_t accounts = 0;
    for (size_t stages = 0; stages < get_registration_schedule().size(); ++stages)
    {
        for (size_t stage_accounts = 0; stage_accounts < get_registration_schedule()[stages].users;
             ++stage_accounts, ++accounts)
        {
            BOOST_CHECK_EQUAL((share_type)get_registration_schedule()[stages].bonus_percent,
                              schedule_input_bonus_percent_by_pos(accounts));
        }
    }
}

SCORUM_TEST_CASE(schedule_input_predicted_allocate_by_pos_check)
{
    asset maximum_bonus(5, SCORUM_SYMBOL);

    size_t pos = 0;
    for (size_t stages = 0; stages < get_registration_schedule().size(); ++stages)
    {
        BOOST_CHECK_EQUAL((share_type)get_registration_schedule()[stages].bonus_percent * maximum_bonus.amount / 100,
                          schedule_input_predicted_allocate_by_pos(pos, maximum_bonus).amount);
        pos += get_registration_schedule()[stages].users;
    }
}

SCORUM_TEST_CASE(schedule_input_pos_reach_limit_check)
{
    asset maximum_bonus(5, SCORUM_SYMBOL);

    uint64_t pos = 0;
    uint64_t max_pos = schedule_input_max_pos();
    asset predicted_limit = maximum_bonus * 2;
    auto fn = [this, maximum_bonus](uint64_t p) -> asset {
        return this->schedule_input_predicted_allocate_by_pos(p, maximum_bonus);
    };

    BOOST_REQUIRE(schedule_input_pos_reach_limit(pos, max_pos, predicted_limit, maximum_bonus, fn));

    BOOST_REQUIRE_LT(pos, max_pos);
}

BOOST_AUTO_TEST_SUITE_END()
}