void FunctionHeadToElementVelocity<T>::finalizeTimeStep(const NumLib::TimeStep &time)
{
    //std::cout << "Velocity=" << std::endl;
    //_vel->printout();
    //update data for output
    Ogs6FemData* femData = Ogs6FemData::getInstance();
    OutputVariableInfo var(this->getOutputParameterName(Velocity), _dis->getMesh()->getID(), OutputVariableInfo::Element, OutputVariableInfo::Real, 3, _vel);
    femData->outController.setOutput(var.name, var);

    _tim->finalize(time.getTime());
};
void FemPoroelasticResidualLocalAssembler::assembleComponents
    (   const NumLib::TimeStep &timestep,
        const MeshLib::IElement &e, 
        const std::vector<size_t> &vec_order, 
        const std::vector<LocalVectorType> &vec_x0,
        const std::vector<LocalVectorType> &vec_x1,
        std::vector<LocalVectorType> &vec_r
        )
{
    assert(vec_order.size()==3);

    //TODO how do you know 1st is ux and 3rd is p? who decides it?
    const size_t id_ux = 0;
    const size_t id_uy = 1;
    const size_t id_p = 2;

    const size_t u_order = vec_order[id_ux];
    assert(u_order==vec_order[id_uy]);
    const size_t p_order = vec_order[id_p];
    const LocalVectorType &ux0 = vec_x0[id_ux];
    const LocalVectorType &uy0 = vec_x0[id_uy];
    const LocalVectorType &ux1 = vec_x1[id_ux];
    const LocalVectorType &uy1 = vec_x1[id_uy];
    // combine ux and uy
    LocalVectorType u0(ux0.rows()*2);
    LocalVectorType u1(ux0.rows()*2);
    for (int i=0; i<ux0.rows(); i++) {
        u0(i) = ux0(i);
        u0(i+ux0.rows()) = uy0(i);
        u1(i) = ux1(i);
        u1(i+ux0.rows()) = uy1(i);
    }
    const LocalVectorType &p0 = vec_x0[id_p];
    const LocalVectorType &p1 = vec_x1[id_p];
    // ------------------------------------------------------------------------
    // Element
    // ------------------------------------------------------------------------
    const size_t dim = e.getDimension();
    const size_t n_strain_components = getNumberOfStrainComponents(dim);
    const size_t nnodes_u = e.getNumberOfNodes(u_order);
    const size_t nnodes_p = e.getNumberOfNodes(p_order);
    const NumLib::TXPosition e_pos(NumLib::TXPosition::Element, e.getID());

    // ------------------------------------------------------------------------
    // Transient
    // ------------------------------------------------------------------------
    const double dt = timestep.getTimeStepSize();
    const double theta = 1.0;

    // ------------------------------------------------------------------------
    // Material (assuming element constant)
    // ------------------------------------------------------------------------
    size_t mat_id = e.getGroupID();
    size_t fluid_id = 0; //TODO
    Ogs6FemData* femData = Ogs6FemData::getInstance();
    MaterialLib::PorousMedia* pm = femData->list_pm[mat_id];
    MaterialLib::Solid *solidphase = femData->list_solid[mat_id];
    MaterialLib::Fluid *fluidphase = femData->list_fluid[fluid_id];

    // solid
    double rho_s = .0;
    if (solidphase->density!=NULL)
        solidphase->density->eval(e_pos, rho_s);
    LocalMatrixType De = LocalMatrixType::Zero(n_strain_components, n_strain_components);
    MathLib::LocalMatrix nv(1,1);
    MathLib::LocalMatrix E(1,1);
    solidphase->poisson_ratio->eval(e_pos, nv);
    solidphase->Youngs_modulus->eval(e_pos, E);
    double Lambda, G, K;
    MaterialLib::calculateLameConstant(nv(0,0), E(0,0), Lambda, G, K);
    MaterialLib::setElasticConsitutiveTensor(dim, Lambda, G, De);

    // fluid
    double mu = .0;
    fluidphase->dynamic_viscosity->eval(e_pos, mu);
    double rho_f = .0;
    fluidphase->density->eval(e_pos, rho_f);

    // media
    double k;
    pm->permeability->eval(e_pos, k);
    double n = .0;
    pm->porosity->eval(e_pos, n);
    double s = .0;
    pm->storage->eval(e_pos, s);
    double k_mu;
    k_mu = k / mu;


    // ------------------------------------------------------------------------
    // Body force
    // ------------------------------------------------------------------------
    LocalVectorType body_force = LocalVectorType::Zero(dim);
    bool hasGravity = false;
    if (hasGravity) {
        body_force[dim-1] = rho_s * 9.81;
    }

    // ------------------------------------------------------------------------
    // Local component assembly
    // ------------------------------------------------------------------------
    LocalMatrixType Kuu = LocalMatrixType::Zero(nnodes_u*dim, nnodes_u*dim);
    LocalMatrixType Cup = LocalMatrixType::Zero(nnodes_u*dim, nnodes_p);
    LocalMatrixType Kpp = LocalMatrixType::Zero(nnodes_p, nnodes_p);
    LocalMatrixType Mpp = LocalMatrixType::Zero(nnodes_p, nnodes_p);
    LocalMatrixType Cpu = LocalMatrixType::Zero(nnodes_p, nnodes_u*dim);
    LocalVectorType Fu = LocalVectorType::Zero(nnodes_u*dim);
    LocalVectorType Fp = LocalVectorType::Zero(nnodes_p);

    // temp matrix
    LocalMatrixType B = LocalMatrixType::Zero(n_strain_components, nnodes_u*dim);
    LocalMatrixType Nuvw = LocalMatrixType::Zero(dim, nnodes_u*dim);
    const LocalMatrixType m = get_m(dim);

    //
    FemLib::IFiniteElement* fe_u = _feObjects.getFeObject(e, u_order);
    FemLib::IFiniteElement* fe_p = _feObjects.getFeObject(e, p_order);
    FemLib::IFemNumericalIntegration *q_u = fe_u->getIntegrationMethod();
    double gp_x[3], real_x[3];
    for (size_t j=0; j<q_u->getNumberOfSamplingPoints(); j++) {
        q_u->getSamplingPoint(j, gp_x);
        fe_u->computeBasisFunctions(gp_x);
        fe_p->computeBasisFunctions(gp_x);
        fe_u->getRealCoordinates(real_x);
        double fac_u = fe_u->getDetJ() * q_u->getWeight(j);

        //--- local component ----
        // set N,B
        LocalMatrixType &Nu = *fe_u->getBasisFunction();
        LocalMatrixType &dNu = *fe_u->getGradBasisFunction();
        setNu_Matrix_byComponent(dim, nnodes_u, Nu, Nuvw);
        setB_Matrix_byComponent(dim, nnodes_u, dNu, B);
        LocalMatrixType &Np = *fe_p->getBasisFunction();

        // K_uu += B^T * D * B
        Kuu.noalias() += fac_u * B.transpose() * De * B;

        // C_up += B^T * m * Np
        Cup.noalias() += fac_u * B.transpose() * m * Np;

        // Fu += N^T * b
        if (hasGravity) {
            Fu.noalias() += fac_u * Nuvw.transpose() * body_force;
        }
    }
    Fu.noalias() += (theta - 1) * Kuu * u0 + (1-theta)* Cup * p0;

    FemLib::IFemNumericalIntegration *q_p = fe_p->getIntegrationMethod();
    for (size_t j=0; j<q_p->getNumberOfSamplingPoints(); j++) {
        q_p->getSamplingPoint(j, gp_x);
        fe_u->computeBasisFunctions(gp_x);
        fe_p->computeBasisFunctions(gp_x);
        fe_p->getRealCoordinates(real_x);
        double fac = fe_p->getDetJ() * q_p->getWeight(j);

        //--- local component ----
        // set N,B
        LocalMatrixType &dNu = *fe_u->getGradBasisFunction();
        setB_Matrix_byComponent(dim, nnodes_u, dNu, B);
        LocalMatrixType &Np = *fe_p->getBasisFunction();
        LocalMatrixType &dNp = *fe_p->getGradBasisFunction();

        // M_pp += Np^T * S * Np
        Mpp.noalias() += fac * Np.transpose() * s * Np;

        // K_pp += dNp^T * K * dNp
        Kpp.noalias() += fac * dNp.transpose() * k_mu * dNp;

        // C_pu += Np^T * m^T * B
        Cpu.noalias() += fac * Np.transpose() * m.transpose() * B;
    }

    // Backward euler
    Fp = (1.0/dt * Mpp - (1-theta)*Kpp)* p0 + 1.0/dt * Cpu * u0;

    // r = K*u - RHS
    LocalVectorType r_u = Kuu * u1 - Cup * p1 - Fu;
    LocalVectorType r_p = 1.0/dt * Cpu * u1 + (1.0/dt * Mpp + theta * Kpp) * p1 - Fp;
//    if (e.getID()==0) {
//        std::cout << "u1=" << std::endl << u1 << std::endl;
//        std::cout << "p1=" << std::endl << p1 << std::endl;
//        std::cout << "Fp=" << std::endl << Fp << std::endl;
//        std::cout << "r_p=" << std::endl << r_p << std::endl;
//    }

    //
    for (size_t i=0; i<dim; i++) {
        vec_r[i] = r_u.segment(i*nnodes_u, nnodes_u);
    }
    vec_r[id_p] = r_p;
}
TEST(NumLib, TimeSteppingIterationNumberBased1)
{
    std::vector<std::size_t> iter_times_vector = {0, 3, 5, 7};
    std::vector<double> multiplier_vector = {2.0, 1.0, 0.5, 0.25};
    NumLib::IterationNumberBasedAdaptiveTimeStepping alg(1, 31, 1, 10, 1, iter_times_vector, multiplier_vector);

    ASSERT_TRUE(alg.next()); // t=2, dt=1
    NumLib::TimeStep ts = alg.getTimeStep();
    ASSERT_EQ(1u, ts.steps());
    ASSERT_EQ(1., ts.previous());
    ASSERT_EQ(2., ts.current());
    ASSERT_EQ(1., ts.dt());
    ASSERT_TRUE(alg.accepted());

    ASSERT_TRUE(alg.next()); // t=4, dt=2

    // dt*=2
    alg.setNIterations(3);
    ASSERT_TRUE(alg.next()); // t=8, dt=4
    ts = alg.getTimeStep();
    ASSERT_EQ(3u, ts.steps());
    ASSERT_EQ(4., ts.previous());
    ASSERT_EQ(8., ts.current());
    ASSERT_EQ(4., ts.dt());
    ASSERT_TRUE(alg.accepted());

    // dt*=1
    alg.setNIterations(5);
    ASSERT_TRUE(alg.next()); // t=12, dt=4
    ts = alg.getTimeStep();
    ASSERT_EQ(4u, ts.steps());
    ASSERT_EQ(8., ts.previous());
    ASSERT_EQ(12., ts.current());
    ASSERT_EQ(4., ts.dt());
    ASSERT_TRUE(alg.accepted());

    // dt*=0.5
    alg.setNIterations(7);
    ASSERT_TRUE(alg.next()); // t=14, dt=2
    ts = alg.getTimeStep();
    ASSERT_EQ(5u, ts.steps());
    ASSERT_EQ(12., ts.previous());
    ASSERT_EQ(14., ts.current());
    ASSERT_EQ(2., ts.dt());
    ASSERT_TRUE(alg.accepted());

    // dt*=0.25 but dt_min = 1
    alg.setNIterations(8); // exceed max
    ASSERT_TRUE(alg.next()); // t=13, dt=1
    ts = alg.getTimeStep();
    ASSERT_EQ(5u, ts.steps());
    ASSERT_EQ(12., ts.previous());
    ASSERT_EQ(13, ts.current());
    ASSERT_EQ(1., ts.dt());
    ASSERT_FALSE(alg.accepted());

    // restart, dt*=1
    alg.setNIterations(4);
    ASSERT_TRUE(alg.next()); // t=14, dt=1
    ts = alg.getTimeStep();
    ASSERT_EQ(6u, ts.steps());
    ASSERT_EQ(13., ts.previous());
    ASSERT_EQ(14, ts.current());
    ASSERT_EQ(1., ts.dt());
    ASSERT_TRUE(alg.accepted());
}
TEST(NumLibTimeStepping, testEvolutionaryPIDcontroller)
{
    const char xml[] =
        "<time_stepping>"
        "   <type>EvolutionaryPIDcontroller</type>"
        "   <t_initial> 0.0 </t_initial>"
        "   <t_end> 10 </t_end>"
        "   <dt_guess> 0.01 </dt_guess>"
        "   <dt_min> 0.001 </dt_min>"
        "   <dt_max> 1 </dt_max>"
        "   <rel_dt_min> 0.01 </rel_dt_min>"
        "   <rel_dt_max> 5 </rel_dt_max>"
        "   <tol> 1.e-3 </tol>"
        "</time_stepping>";
    auto const PIDStepper = createTestTimeStepper(xml);

    double solution_error = 0.;
    int const number_iterations = 0;
    // 1st step
    ASSERT_TRUE(PIDStepper->next(solution_error, number_iterations));
    NumLib::TimeStep ts = PIDStepper->getTimeStep();
    double h_new = 0.01;
    double t_previous = 0.;
    ASSERT_EQ(1u, ts.steps());
    ASSERT_EQ(t_previous, ts.previous());
    ASSERT_EQ(t_previous + h_new, ts.current());
    ASSERT_EQ(h_new, ts.dt());
    ASSERT_TRUE(PIDStepper->accepted());
    t_previous += h_new;

    // e_n_minus1 is filled.
    solution_error = 1.0e-4;
    PIDStepper->next(solution_error, number_iterations);
    ts = PIDStepper->getTimeStep();
    h_new = ts.dt();
    ASSERT_EQ(2u, ts.steps());
    const double tol = 1.e-16;
    ASSERT_NEAR(t_previous, ts.previous(), tol);
    ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
    ASSERT_TRUE(PIDStepper->accepted());
    t_previous += h_new;

    // e_n_minus2 is filled.
    solution_error = 0.5e-3;
    PIDStepper->next(solution_error, number_iterations);
    ts = PIDStepper->getTimeStep();
    h_new = ts.dt();
    ASSERT_EQ(3u, ts.steps());
    ASSERT_NEAR(t_previous, ts.previous(), tol);
    ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
    ASSERT_TRUE(PIDStepper->accepted());

    // error > TOL=1.3-3, step rejected and new step size estimated.
    solution_error = 0.01;
    PIDStepper->next(solution_error, number_iterations);
    ts = PIDStepper->getTimeStep();
    h_new = ts.dt();
    // No change in ts.steps
    ASSERT_EQ(3u, ts.steps());
    // No change in ts.previous(), which is the same as that of the previous
    // step.
    ASSERT_NEAR(t_previous, ts.previous(), tol);
    ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
    ASSERT_FALSE(PIDStepper->accepted());
    t_previous += h_new;

    // With e_n, e_n_minus1, e_n_minus2
    solution_error = 0.4e-3;
    PIDStepper->next(solution_error, number_iterations);
    ts = PIDStepper->getTimeStep();
    h_new = ts.dt();
    ASSERT_EQ(4u, ts.steps());
    ASSERT_NEAR(t_previous, ts.previous(), tol);
    ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
    ASSERT_TRUE(PIDStepper->accepted());
}