diff --git a/src/dirichlet.jl b/src/dirichlet.jl
index 333726c..cb7e218 100644
--- a/src/dirichlet.jl
+++ b/src/dirichlet.jl
@@ -53,19 +53,19 @@ function assemble!(assembly::Assembly, problem::Problem{Dirichlet}, element::Ele
             w *= norm(cross(JT[:,1], JT[:,2]))
         end
         N = element(ip, time)
-        Phi = (Ae*N')'
 
-        g_prev = element(field_name, ip, time)
-        #info("g_prev = $g_prev")
         for i=1:field_dim
             ldofs = gdofs[i:field_dim:end]
             if haskey(element, field_name*" $i")
                 g = element(field_name*" $i", ip, time)
                 if get_formulation_type(problem) == :incremental
-                    g = g - g_prev[i]
+                    # if having incremental formulation need to add previous
+                    # displacement to rhs (solving increment Δu !
+                    haskey(element, "displacement") || continue
+                    g_prev = element(field_name, ip, time)
+                    g -= g_prev[i]
                 end
-                #info("g_new = $g")
-                add!(assembly.g, ldofs, w*g*Phi')
+                add!(assembly.g, ldofs, w*g*Ae*N')
             end
         end
 
diff --git a/src/fields.jl b/src/fields.jl
index 61681bd..9accf28 100644
--- a/src/fields.jl
+++ b/src/fields.jl
@@ -306,7 +306,10 @@ function Base.call(field::DCTV, time::Real)
         t0 = field[i-1].time
         t1 = field[i].time
         if t0 < time < t1
-            new_data = field[i-1].data + (time-t0)/(t1-t0)*field[i].data
+            y0 = field[i-1].data
+            y1 = field[i].data
+            dt = t1-t0
+            new_data = y0*(1-(time-t0)/dt) + y1*(1-(t1-time)/dt)
             return DCTI(new_data)
         end
     end
@@ -323,7 +326,10 @@ function Base.call(field::DVTV, time::Float64)
         t0 = field[i-1].time
         t1 = field[i].time
         if t0 < time < t1
-            new_data = field[i-1].data + (time-t0)/(t1-t0)*field[i].data
+            y0 = field[i-1].data
+            y1 = field[i].data
+            dt = t1-t0
+            new_data = y0*(1-(time-t0)/dt) + y1*(1-(t1-time)/dt)
             return DVTI(new_data)
         end
     end
diff --git a/src/mortar_2d_autodiff.jl b/src/mortar_2d_autodiff.jl
index 7b696f3..f638f8f 100644
--- a/src/mortar_2d_autodiff.jl
+++ b/src/mortar_2d_autodiff.jl
@@ -2,38 +2,33 @@
 # License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md
 
 """ Find segment from slave element corresponding to master element nodes.
-x1_, n1_
-slave element geometry and normal direction
 
-x2_ master element nodes to project onto slave
+Parameters
+----------
+x1_, n1_
+    slave element geometry and normal direction
+x2
+    master element node to project onto slave
+
+Returns
+-------
+xi
+    dimensionless coordinate on slave corresponding to
+    projected master
+ 
 """
 function project_from_master_to_slave{E<:MortarElements2D}(
     slave_element::Element{E}, x1_::DVTI, n1_::DVTI, x2::Vector;
     tol=1.0e-10, max_iterations=20)
 
-    function x1(xi1)
-        N = get_basis(E, xi1)
-        return vec(N)*x1_
-    end
-
-    function dx1(xi1)
-        dN = get_dbasis(E, xi1)
-        return vec(dN)*x1_
-    end
-
-    function n1(xi1)
-        N = get_basis(E, xi1)
-        return vec(N)*n1_
-    end
-
-    function dn1(xi1)
-        dN = get_dbasis(E, xi1)
-        return vec(dN)*n1_
-    end
-
+    x1(xi1) = vec(get_basis(E, xi1))*x1_
+    dx1(xi1) = vec(get_dbasis(E, xi1))*x1_
+    n1(xi1) = vec(get_basis(E, xi1))*n1_
+    dn1(xi1) = vec(get_dbasis(E, xi1))*n1_
     cross2(a, b) = cross([a; 0], [b; 0])[3]
     R(xi1) = cross2(x1(xi1)-x2, n1(xi1))
     dR(xi1) = cross2(dx1(xi1), n1(xi1)) + cross2(x1(xi1)-x2, dn1(xi1))
+
     xi1 = 0.0
     dxi1 = 0.0
     for i=1:max_iterations
@@ -58,19 +53,12 @@ function project_from_slave_to_master{E<:MortarElements2D}(
     master_element::Element{E}, x1::Vector, n1::Vector, x2_::DVTI;
     tol=1.0e-10, max_iterations=20)
 
-    function x2(xi2)
-        N = get_basis(E, xi2)
-        return vec(N)*x2_
-    end
-
-    function dx2(xi2)
-        dN = get_dbasis(E, xi2)
-        return vec(dN)*x2_
-    end
-
+    x2(xi2) = vec(get_basis(E, xi2))*x2_
+    dx2(xi2) = vec(get_dbasis(E, xi2))*x2_
     cross2(a, b) = cross([a; 0], [b; 0])[3]
     R(xi2) = cross2(x2(xi2)-x1, n1)
     dR(xi2) = cross2(dx2(xi2), n1)
+
     xi2 = 0.0
     dxi2 = 0.0
     for i=1:max_iterations
@@ -127,8 +115,7 @@ function assemble!{E<:MortarElements2D}(assembly::Assembly,
             end
         end
         # --> slave side normals in deformed state
-        n1 = Field(Vector[ForwardDiff.get_value(normals[:,i]/norm(normals[:,i])) for i in slave_element_nodes])
-
+        n1 = Field(Vector[normals[:,i]/norm(normals[:,i]) for i in slave_element_nodes])
 
         nnodes = size(slave_element, 2)
         lan_tot = zeros(nnodes) # normal pressure
@@ -206,19 +193,16 @@ function assemble!{E<:MortarElements2D}(assembly::Assembly,
             n = n1[i]
             t = Q'*n
             R = [n t]
-            la_nt = R*la[:,j]
-            info("node $j, n=$(ForwardDiff.get_value(n)) lan = $(ForwardDiff.get_value(la_nt[1])) gap = $(ForwardDiff.get_value(gap_tot[i]))")
+            la_nt = R'*la[:,j]
+#           info("node $j, n=$(ForwardDiff.get_value(n)) lan = $(ForwardDiff.get_value(la_nt[1])) gap = $(ForwardDiff.get_value(gap_tot[i]))")
 
-#           if -lan_tot[i] + gap_tot[i] < 0
-            if -la_nt[1] + gap_tot[i] < 0
+            if la_nt[1] - gap_tot[i] > 0
                 info("set node $j active")
                 C[1,j] += gap_tot[i]
-#               C[1,j] += la_nt[1] - max(0, la_nt[1] - gap_tot[i])
                 C[2,j] += la_nt[2]
             else
                 info("set node $j inactive")
-                C[1,j] += la1[i][1]
-                C[2,j] += la1[i][2]
+                C[:,j] = la[:,j]
             end
         end
 
diff --git a/src/problems.jl b/src/problems.jl
index 991d5b1..0c65b6a 100644
--- a/src/problems.jl
+++ b/src/problems.jl
@@ -164,6 +164,9 @@ function update_assembly!(problem, u, la)
     end
 
     # copy current solutions to previous ones and add/replace new solution
+    # TODO: here we have couple of options and they needs to be clarified
+    # for total formulation we are solving total quantity Ku=f while in
+    # incremental formulation we solve KΔu = f and u = u + Δu
     assembly.u_prev = copy(assembly.u)
     assembly.la_prev = copy(assembly.la)
     if get_formulation_type(problem) == :total
@@ -173,11 +176,11 @@ function update_assembly!(problem, u, la)
     elseif get_formulation_type(problem) == :incremental
         info("$(problem.name): incremental formulation, adding increment to solution vector")
         assembly.u += u
-        assembly.la = la
+        assembly.la += la
     elseif get_formulation_type(problem) == :forwarddiff
         info("$(problem.name): forwarddiff formulation, adding increment to solution vector")
         assembly.u += u
-        assembly.la = la
+        assembly.la += la
     else
         info("$(problem.name): unknown formulation type, don't know what to do with results")
         error("serious failure with problem formulation: $(get_formulation_type(problem))")
diff --git a/src/solvers.jl b/src/solvers.jl
index 31a172a..cb1aa34 100644
--- a/src/solvers.jl
+++ b/src/solvers.jl
@@ -1,55 +1,6 @@
 # This file is a part of JuliaFEM.
 # License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md
 
-"""
-Solve field equations for a single problem with some dofs fixed. This can be used
-to test nonlinear element formulations. Dirichlet boundary is assumed to be homogeneous
-and degrees of freedom are eliminated. So if boundary condition is known in nodal
-points and everything is zero this should be quite good.
-"""
-function solve!(problem::Problem, free_dofs::Vector{Int}, time::Float64; max_iterations::Int=10, tolerance::Float64=1.0e-12, dump_matrices::Bool=false, callback=nothing)
-    info("start solver")
-    assembly = Assembly()
-#   x = zeros(ga.ndofs)
-#   dx = fill!(similar(x), 0.0)
-#   FIXME: better.
-    x = nothing
-    dx = nothing
-    field_name = get_unknown_field_name(problem)
-    dim = get_unknown_field_dimension(problem)
-    for i=1:max_iterations
-        assemble!(assembly, problem, time)
-        A = sparse(assembly.stiffness_matrix)
-        b = sparse(assembly.force_vector)
-        if dump_matrices
-            dump(full(A))
-            dump(full(b)')
-        end
-        if isa(dx, Void)
-            x = zeros(length(b))
-            dx = zeros(length(b))
-        end
-        dx[free_dofs] = lufact(A[free_dofs,free_dofs]) \ full(b)[free_dofs]
-        info("Difference in solution norm: $(norm(dx))")
-        x += dx
-        if !(isa(callback, Void))
-            callback(x)
-        end
-        for element in get_elements(problem)
-            gdofs = get_gdofs(element, problem.dim)
-            data = full(x[gdofs])
-            if length(data) != length(element)
-                data = reshape(data, problem.dim, length(element))
-                data = [data[:,i] for i=1:size(data,2)]
-            end
-            push!(element[field_name], time => data)
-        end
-        norm(dx) < tolerance && return
-    end
-    error("Did not converge in $max_iterations iterations")
-end
-
-
 """ Simple linear solver for educational purposes. """
 type LinearSolver
     name :: ASCIIString
@@ -281,17 +232,19 @@ crosspoints.
 function get_boundary_assembly(solver::Solver)
     ndofs = solver.ndofs
     @assert ndofs != 0
-    Kc = spzeros(ndofs, ndofs)
+    K = spzeros(ndofs, ndofs)
     C1 = spzeros(ndofs, ndofs)
     C2 = spzeros(ndofs, ndofs)
     D = spzeros(ndofs, ndofs)
+    f = spzeros(ndofs, 1)
     g = spzeros(ndofs, 1)
     for problem in get_boundary_problems(solver)
         assembly = problem.assembly
-        Kc_ = sparse(assembly.K, ndofs, ndofs)
+        K_ = sparse(assembly.K, ndofs, ndofs)
         C1_ = sparse(assembly.C1, ndofs, ndofs)
         C2_ = sparse(assembly.C2, ndofs, ndofs)
         D_ = sparse(assembly.D, ndofs, ndofs)
+        f_ = sparse(assembly.f, ndofs, 1)
         g_ = sparse(assembly.g, ndofs, 1)
         # check for overconstraint situation and handle it if possible
         already_constrained = get_nonzero_rows(C2)
@@ -303,13 +256,14 @@ function get_boundary_assembly(solver::Solver)
             handle_overconstraint_error!(problem, overconstrained_nodes,
                 overconstrained_dofs, C1, C1_, C2, C2_, D, D_, g, g_)
         end
-        Kc += Kc_
+        K += K_
         C1 += C1_
         C2 += C2_
         D += D_
+        f += f_
         g += g_
     end
-    return Kc, C1, C2, D, g
+    return K, C1, C2, D, f, g
 end
 
 
@@ -323,11 +277,11 @@ function solve_linear_system(solver::Solver, ::Type{Val{:DirectLinearSolver}})
     K, f = get_field_assembly(solver)
 
     # assemble boundary problems
-    Kc, C1, C2, D, g = get_boundary_assembly(solver)
+    Kb, C1, C2, D, fb, g = get_boundary_assembly(solver)
 
     # construct global system Ax=b and solve using lu factorization
-    A = [K+Kc C1'; C2 D]
-    b = [f; g]
+    A = [K+Kb C1'; C2 D]
+    b = [f+fb; g]
 
     nz = get_nonzero_rows(A)
     x = zeros(length(b))
@@ -394,6 +348,8 @@ function call(solver::Solver)
 
     # 2. start non-linear iterations
     for solver.iteration=1:solver.nonlinear_system_max_iterations
+        info("Starting nonlinear iteration #$(solver.iteration)")
+
         # 2.1 update linearized assemblies (if needed)
         for problem in solver.problems
             problem.assembly.changed = true  # force reassembly
@@ -426,3 +382,4 @@ function call(solver::Solver)
         throw(NonlinearConvergenceError(solver))
     end
 end
+
diff --git a/test/test_dirichlet.jl b/test/test_dirichlet.jl
index 381199f..d3a7de4 100644
--- a/test/test_dirichlet.jl
+++ b/test/test_dirichlet.jl
@@ -1,39 +1,52 @@
 # This file is a part of JuliaFEM.
 # License is MIT: see https://github.com/JuliaFEM/JuliaFEM.jl/blob/master/LICENSE.md
 
-module TestDirichletBoundaryCondition
-
 using JuliaFEM.Test
-using JuliaFEM.Core: Tri3, Seg2, DirichletProblem, Assembly, assemble, Node,
-                     BiorthogonalBasis
-
-@testset "test dirichlet boundary conditions" begin
+using JuliaFEM.Core: Tri3, Seg2, Dirichlet, Assembly, assemble!, Node, Problem
 
+#=
 @testset "dirichlet problem in 1 dimension" begin
     element = Seg2([1, 2])
     element["geometry"] = Node[[1.0, 1.0], [0.0, 1.0]]
     element["temperature"] = 0.0
-    problem = DirichletProblem("temperature", 1)
+    problem = Problem(Dirichlet, "test problem", 1, "temperature")
     push!(problem, element)
-    assembly = assemble(problem, 0.0)
-    C1 = full(assembly.C1)
-    C2 = full(assembly.C2)
-    g = full(assembly.g)
+    assemble!(problem, 0.0)
+    C1 = full(problem.assembly.C1)
+    info("C1")
+    dump(C1)
+    C2 = full(problem.assembly.C2)
+    g = full(problem.assembly.g)
     @test isapprox(C1, C2)
     @test isapprox(C1, 1/6*[2 1; 1 2])
     @test isapprox(g, [0.0, 0.0])
 end
 
+@testset "dirichlet problem using tri3 surface element" begin
+    element = Tri3([1, 2, 3])
+    element["geometry"] = Node[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]
+    element["temperature"] = 0.0
+    problem = Problem(Dirichlet, "test problem", 1, "temperature")
+    push!(problem, element)
+    assemble!(problem, 0.0)
+    C1 = full(problem.assembly.C1)
+    C2 = full(problem.assembly.C2)
+    @test isapprox(C1, C2)
+    @test isapprox(C1, 1/24*[2 1 1; 1 2 1; 1 1 2])
+end
+=#
+
 @testset "dirichlet problem in 2 dimensions" begin
     element = Seg2([1, 2])
     element["geometry"] = Node[[1.0, 1.0], [0.0, 1.0]]
-    element["displacement"] = 0.0
-    problem = DirichletProblem("displacement", 2)
+    element["displacement 1"] = 0.0
+    element["displacement 2"] = 0.0
+    problem = Problem(Dirichlet, "test problem", 2, "displacement")
     push!(problem, element)
-    assembly = assemble(problem, 0.0)
-    C1 = full(assembly.C1)
-    C2 = full(assembly.C2)
-    g = full(assembly.g)
+    assemble!(problem, 0.0)
+    C1 = full(problem.assembly.C1)
+    C2 = full(problem.assembly.C2)
+    g = full(problem.assembly.g)
     @test isapprox(C1, C2)
     C1_expected = 1/6*[2 0 1 0; 0 2 0 1; 1 0 2 0; 0 1 0 2]
     @test isapprox(C1, C1_expected)
@@ -44,12 +57,12 @@ end
     element = Seg2([1, 2])
     element["geometry"] = Node[[1.0, 1.0], [0.0, 1.0]]
     element["displacement 2"] = 0.0
-    problem = DirichletProblem("displacement", 2)
+    problem = Problem(Dirichlet, "test problem", 2, "displacement")
     push!(problem, element)
-    assembly = assemble(problem, 0.0)
-    C1 = full(assembly.C1)
-    C2 = full(assembly.C2)
-    g = full(assembly.g)
+    assemble!(problem, 0.0)
+    C1 = full(problem.assembly.C1)
+    C2 = full(problem.assembly.C2)
+    g = full(problem.assembly.g)
     @test isapprox(C1, C2)
     C1_expected = 1/6*[
         0 0 0 0
@@ -60,50 +73,3 @@ end
     @test isapprox(g, [0.0, 0.0, 0.0, 0.0])
 end
 
-@testset "dirichlet problem using tri3 surface element" begin
-    elem = Tri3([1, 2, 3])
-    elem["geometry"] = Node[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]
-    elem["temperature"] = 0.0
-    prob = DirichletProblem("temperature", 1)
-    push!(prob, elem)
-    ass = assemble(prob, 0.0)
-    C1 = full(ass.C1)
-    C2 = full(ass.C2)
-    @test isapprox(C1, C2)
-    @test isapprox(C1, 1/24*[2 1 1; 1 2 1; 1 1 2])
-end
-
-@testset "dirichlet problem using biorthogonal basis" begin
-    elem = Tri3([1, 2, 3])
-    elem["geometry"] = Node[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]
-    elem["displacement 1"] = 1.0
-    prob = DirichletProblem("displacement", 3; basis=BiorthogonalBasis)
-    push!(prob, elem)
-    ass = assemble(prob, 0.0)
-    C1 = full(ass.C1, 9, 9)
-    C2 = full(ass.C2, 9, 9)
-    D = full(ass.D, 9, 9)
-    g = full(ass.g, 9, 1)
-    C1_expected = eye(9)*1.0/6.0
-    g_expected = zeros(9)
-    g_expected[1] = g_expected[4] = g_expected[7] = 1.0/6.0
-    C2_expected = zeros(9, 9)
-    D_expected = zeros(9, 9)
-    C2_expected[1, 1] = 1.0/6.0
-    D_expected[2, 2] = 1.0/6.0
-    D_expected[3, 3] = 1.0/6.0
-    C2_expected[4, 4] = 1.0/6.0
-    D_expected[5, 5] = 1.0/6.0
-    D_expected[6, 6] = 1.0/6.0
-    C2_expected[7, 7] = 1.0/6.0
-    D_expected[8, 8] = 1.0/6.0
-    D_expected[9, 9] = 1.0/6.0
-    @test isapprox(C1, C1_expected)
-    @test isapprox(C2, C2_expected)
-    @test isapprox(D, D_expected)
-    @test isapprox(g, g_expected)
-end
-
-end
-
-end
diff --git a/test/test_fields_time_interpolation.jl b/test/test_fields_time_interpolation.jl
index bf8c303..217d9fb 100644
--- a/test/test_fields_time_interpolation.jl
+++ b/test/test_fields_time_interpolation.jl
@@ -16,4 +16,11 @@ using JuliaFEM.Core: DCTV, DCTI
     @test isapprox(f( 0.9), DCTI(0.9))
     @test isapprox(f( 1.0), DCTI(1.0))
     @test isapprox(f( 1.5), DCTI(1.0))
+
+    f2 = DCTV(0.0 => 0.0, 0.25 => -0.1, 0.50 => -0.1)
+    @test isapprox(f2(0.0), DCTI(0.0))
+    @test isapprox(f2(0.25), DCTI(-0.1))
+    @test isapprox(f2(0.50), DCTI(-0.1))
+    @test isapprox(f2(0.35), DCTI(-0.1))
 end
+