void 
UZRectMollerup::drain (const GeometryRect& geo,
                       const std::vector<size_t>& drain_cell,
		       const double drain_water_level,
                       const ublas::vector<double>& h,
                       const ublas::vector<double>& Theta_previous,
                       const ublas::vector<double>& Theta,
                       const ublas::vector<double>& S_vol,
#ifdef TEST_OM_DEN_ER_BRUGT
                       const ublas::vector<double>& S_macro,
#endif
                       const ublas::vector<double>& dq,
                       const double& ddt,
                       std::vector<bool>& drain_cell_on,
                       Solver::Matrix& A,
                       ublas::vector<double>& b,
                       const int debug, Treelog& msg)
{
  const size_t drain_size  = drain_cell.size (); // // number of drains   
    
  std::ostringstream tmp;

  for (size_t d = 0; d < drain_size; d++)
    {
      const size_t cell = drain_cell[d];

      // Pressure in drain cell [cm].
      const double drain_h = drain_water_level - geo.cell_z (cell); 
      
      if (drain_cell_on[d])    //drain on
        {
          //Calculate fluxes to drain from last timestep 
          
          double drain_sink = Theta_previous (cell);
          drain_sink -= Theta (cell);
          drain_sink -= ddt * (S_vol (cell)
#ifdef TEST_OM_DEN_ER_BRUGT
                               + S_macro (cell)
#endif
                               )/
            geo.cell_volume (cell);
                    
          const std::vector<size_t>& edges = geo.cell_edges (cell);
          const size_t edge_size = edges.size ();
          for (size_t i = 0; i < edge_size; i++)
            {
              const size_t edge = edges[i]; 
              const double flux = dq (edge) * geo.edge_area (edge) * ddt;
              const int from = geo.edge_from (edge);
              const int to = geo.edge_to (edge);
              
              if (cell == from)
                drain_sink  -= flux / geo.cell_volume (cell);
              else if (cell == to)
                drain_sink  += flux / geo.cell_volume (cell); 
            }
          if (drain_sink <= 0.0)
            drain_cell_on[d] = false;
        }
      else			// drain off
	if (h (cell) > 0.0)
	  drain_cell_on[d] = true;   	
          
      if (drain_h > 0.0 || drain_cell_on[d] == true)
        {
          const std::vector<size_t>& edges = geo.cell_edges (cell);
          const size_t edge_size = edges.size ();
          
          // Force pressure to be zero.
          for (size_t i = 0; i < edge_size; i++)
            {
              const size_t edge =  edges[i];
              if (!geo.edge_is_internal (edge))
                continue;
              
              const size_t other = geo.edge_other (edge, cell);
              A (cell, other) = 0.0;
            }
          A (cell, cell) = 1.0;
          b (cell) = std::max (drain_h, 0.0);
        }
    }
}
void 
UZRectMollerup::tick (const GeometryRect& geo,
		      const std::vector<size_t>& drain_cell,
		      const double drain_water_level,
		      const Soil& soil, 
		      SoilWater& soil_water, const SoilHeat& soil_heat,
		      const Surface& surface, const Groundwater& groundwater,
		      const double dt, Treelog& msg)

{
  daisy_assert (K_average.get ());
  const size_t edge_size = geo.edge_size (); // number of edges 
  const size_t cell_size = geo.cell_size (); // number of cells 

  // Insert magic here.
  
  ublas::vector<double> Theta (cell_size); // water content 
  ublas::vector<double> Theta_previous (cell_size); // at start of small t-step
  ublas::vector<double> h (cell_size); // matrix pressure
  ublas::vector<double> h_previous (cell_size); // at start of small timestep
  ublas::vector<double> h_ice (cell_size); // 
  ublas::vector<double> S (cell_size); // sink term
  ublas::vector<double> S_vol (cell_size); // sink term
#ifdef TEST_OM_DEN_ER_BRUGT
  ublas::vector<double> S_macro (cell_size);  // sink term
  std::vector<double> S_drain (cell_size, 0.0); // matrix-> macro -> drain flow 
  std::vector<double> S_drain_sum (cell_size, 0.0); // For large timestep
  const std::vector<double> S_matrix (cell_size, 0.0);  // matrix -> macro 
  std::vector<double> S_matrix_sum (cell_size, 0.0); // for large timestep
#endif
  ublas::vector<double> T (cell_size); // temperature 
  ublas::vector<double> Kold (edge_size); // old hydraulic conductivity
  ublas::vector<double> Ksum (edge_size); // Hansen hydraulic conductivity
  ublas::vector<double> Kcell (cell_size); // hydraulic conductivity
  ublas::vector<double> Kold_cell (cell_size); // old hydraulic conductivity
  ublas::vector<double> Ksum_cell (cell_size); // Hansen hydraulic conductivity
  ublas::vector<double> h_lysimeter (cell_size);
  std::vector<bool> active_lysimeter (cell_size);
  const std::vector<size_t>& edge_above = geo.cell_edges (Geometry::cell_above);
  const size_t edge_above_size = edge_above.size ();
  ublas::vector<double> remaining_water (edge_above_size);
  std::vector<bool> drain_cell_on (drain_cell.size (),false); 
  

  for (size_t i = 0; i < edge_above_size; i++)
    {
      const size_t edge = edge_above[i];
      remaining_water (i) = surface.h_top (geo, edge);
    }
  ublas::vector<double> q;	// Accumulated flux
  q = ublas::zero_vector<double> (edge_size);
  ublas::vector<double> dq (edge_size); // Flux in small timestep.
  dq = ublas::zero_vector<double> (edge_size);

  //Make Qmat area diagonal matrix 
  //Note: This only needs to be calculated once... 
  ublas::banded_matrix<double> Qmat (cell_size, cell_size, 0, 0);
  for (int c = 0; c < cell_size; c++)
    Qmat (c, c) = geo.cell_volume (c);
 
  // make vectors 
  for (size_t cell = 0; cell != cell_size ; ++cell) 
    {				
      Theta (cell) = soil_water.Theta (cell);
      h (cell) =  soil_water.h (cell);
      h_ice (cell) = soil_water.h_ice (cell);
      S (cell) =  soil_water.S_sum (cell);
      S_vol (cell) = S (cell) * geo.cell_volume (cell);
      if (use_forced_T)
	T (cell) = forced_T;
      else 
	T (cell) = soil_heat.T (cell); 
      h_lysimeter (cell) = geo.zplus (cell) - geo.cell_z (cell);
    }

  // Remember old value.
  Theta_error = Theta;

  // Start time loop 
  double time_left = dt;	// How much of the large time step left.
  double ddt = dt;		// We start with small == large time step.
  int number_of_time_step_reductions = 0;
  int iterations_with_this_time_step = 0;
  

  int n_small_time_steps = 0;
  
  while (time_left > 0.0)
    {
      if (ddt > time_left)
	ddt = time_left;

      std::ostringstream tmp_ddt;
      tmp_ddt << "Time t = " << (dt - time_left) 
              << "; ddt = " << ddt
              << "; steps " << n_small_time_steps 
              << "; time left = " << time_left;
      Treelog::Open nest (msg, tmp_ddt.str ());

      if (n_small_time_steps > 0
          && (n_small_time_steps%msg_number_of_small_time_steps) == 0)
        {
          msg.touch ();
          msg.flush ();
        }
      
      n_small_time_steps++;
      if (n_small_time_steps > max_number_of_small_time_steps) 
        {
          msg.debug ("Too many small timesteps");
          throw "Too many small timesteps";
        }
      
      // Initialization for each small time step.

      if (debug > 0)
	{
	  std::ostringstream tmp;
	  tmp << "h = " << h << "\n";
	  tmp << "Theta = " << Theta;
	  msg.message (tmp.str ());
	}

      int iterations_used = 0;
      h_previous = h;
      Theta_previous = Theta;

      if (debug == 5)
	{
	  std::ostringstream tmp;
	  tmp << "Remaining water at start: " << remaining_water;
	  msg.message (tmp.str ());
	}

      ublas::vector<double> h_conv;

      for (size_t cell = 0; cell != cell_size ; ++cell)
        active_lysimeter[cell] = h (cell) > h_lysimeter (cell);

      for (size_t edge = 0; edge != edge_size ; ++edge)
        {
          Kold[edge] = find_K_edge (soil, geo, edge, h, h_ice, h_previous, T);
          Ksum [edge] = 0.0;
        }

      std::vector<top_state> state (edge_above.size (), top_undecided);
      
      // We try harder with smaller timesteps.
      const int max_loop_iter 
        = max_iterations * (number_of_time_step_reductions 
                            * max_iterations_timestep_reduction_factor + 1);
      do // Start iteration loop
	{
	  h_conv = h;
	  iterations_used++;
          

          std::ostringstream tmp_conv;
          tmp_conv << "Convergence " << iterations_used; 
          Treelog::Open nest (msg, tmp_conv.str ());
          if (debug == 7)
            msg.touch ();

	  // Calculate conductivity - The Hansen method
	  for (size_t e = 0; e < edge_size; e++)
	    {
              Ksum[e] += find_K_edge (soil, geo, e, h, h_ice, h_previous, T);
              Kedge[e] = (Ksum[e] / (iterations_used  + 0.0)+ Kold[e]) / 2.0;
	    }

	  //Initialize diffusive matrix
	  Solver::Matrix diff (cell_size);
	  // diff = ublas::zero_matrix<double> (cell_size, cell_size);
	  diffusion (geo, Kedge, diff);

	  //Initialize gravitational matrix
	  ublas::vector<double> grav (cell_size); //ublass compatibility
	  grav = ublas::zero_vector<double> (cell_size);
	  gravitation (geo, Kedge, grav);

	  // Boundary matrices and vectors
	  ublas::banded_matrix<double>  Dm_mat (cell_size, cell_size, 
                                                0, 0); // Dir bc
	  Dm_mat = ublas::zero_matrix<double> (cell_size, cell_size);
	  ublas::vector<double>  Dm_vec (cell_size); // Dir bc
	  Dm_vec = ublas::zero_vector<double> (cell_size);
	  ublas::vector<double> Gm (cell_size); // Dir bc
	  Gm = ublas::zero_vector<double> (cell_size);
	  ublas::vector<double> B (cell_size); // Neu bc 
	  B = ublas::zero_vector<double> (cell_size);

	  lowerboundary (geo, groundwater, active_lysimeter, h,
                         Kedge,
                         dq, Dm_mat, Dm_vec, Gm, B, msg);
	  upperboundary (geo, soil, T, surface, state, remaining_water, h,
                         Kedge,
                         dq, Dm_mat, Dm_vec, Gm, B, ddt, debug, msg, dt);
          Darcy (geo, Kedge, h, dq); //for calculating drain fluxes 


	  //Initialize water capacity  matrix
	  ublas::banded_matrix<double> Cw (cell_size, cell_size, 0, 0);
	  for (size_t c = 0; c < cell_size; c++)
	    Cw (c, c) = soil.Cw2 (c, h[c]);
	  
          std::vector<double> h_std (cell_size);
          //ublas vector -> std vector 
          std::copy(h.begin (), h.end (), h_std.begin ());

#ifdef TEST_OM_DEN_ER_BRUGT
          for (size_t cell = 0; cell != cell_size ; ++cell) 
            {				
              S_macro (cell) = (S_matrix[cell] + S_drain[cell]) 
                * geo.cell_volume (cell);
            }
#endif

	  //Initialize sum matrix
	  Solver::Matrix summat (cell_size);  
	  noalias (summat) = diff + Dm_mat;

	  //Initialize sum vector
	  ublas::vector<double> sumvec (cell_size);  
	  sumvec = grav + B + Gm + Dm_vec - S_vol
#ifdef TEST_OM_DEN_ER_BRUGT
            - S_macro
#endif
            ; 

	  // QCw is shorthand for Qmatrix * Cw
	  Solver::Matrix Q_Cw (cell_size);
	  noalias (Q_Cw) = prod (Qmat, Cw);

	  //Initialize A-matrix
	  Solver::Matrix A (cell_size);  
	  noalias (A) = (1.0 / ddt) * Q_Cw - summat;  

	  // Q_Cw_h is shorthand for Qmatrix * Cw * h
	  const ublas::vector<double> Q_Cw_h = prod (Q_Cw, h);

	  //Initialize b-vector
	  ublas::vector<double> b (cell_size);  
	  //b = sumvec + (1.0 / ddt) * (Qmatrix * Cw * h + Qmatrix *(Wxx-Wyy));
	  b = sumvec + (1.0 / ddt) * (Q_Cw_h
				      + prod (Qmat, Theta_previous-Theta));

	  // Force active drains to zero h.
          drain (geo, drain_cell, drain_water_level,
		 h, Theta_previous, Theta, S_vol,
#ifdef TEST_OM_DEN_ER_BRUGT
                 S_macro,
#endif
                 dq, ddt, drain_cell_on, A, b, debug, msg);  
          
          try {
            solver->solve (A, b, h); // Solve Ah=b with regard to h.
          } catch (const char *const error) {
              std::ostringstream tmp;
              tmp << "Could not solve equation system: " << error;
              msg.warning (tmp.str ());
              // Try smaller timestep.
              iterations_used = max_loop_iter + 100;
              break;
          }

	  for (int c=0; c < cell_size; c++) // update Theta 
	    Theta (c) = soil.Theta (c, h (c), h_ice (c)); 

	  if (debug > 1)
	    {
	      std::ostringstream tmp;
	      tmp << "Time left = " << time_left << ", ddt = " << ddt 
		  << ", iteration = " << iterations_used << "\n";
	      tmp << "B = " << B << "\n";
	      tmp << "h = " << h << "\n";
	      tmp << "Theta = " << Theta;
	      msg.message (tmp.str ());
	    }
          
          for (int c=0; c < cell_size; c++)
            {
              if (h (c) < min_pressure_potential || h (c) > max_pressure_potential)
                {
                  std::ostringstream tmp;
                  tmp << "Pressure potential out of realistic range, h[" 
                      << c << "] = " << h (c);
                  msg.debug (tmp.str ());
                  iterations_used = max_loop_iter + 100;
                  break;
                } 
            }
        }

      while (!converges (h_conv, h) && iterations_used <= max_loop_iter);
      

      if (iterations_used > max_loop_iter)
	{
          number_of_time_step_reductions++;
          
	  if (number_of_time_step_reductions > max_time_step_reductions)
            {
              msg.debug ("Could not find solution");
              throw "Could not find solution";
            }

          iterations_with_this_time_step = 0;
	  ddt /= time_step_reduction;
	  h = h_previous;
	  Theta = Theta_previous;
	}
      else
	{
          // Update dq for new h.
	  ublas::banded_matrix<double>  Dm_mat (cell_size, cell_size, 
                                                0, 0); // Dir bc
	  Dm_mat = ublas::zero_matrix<double> (cell_size, cell_size);
	  ublas::vector<double>  Dm_vec (cell_size); // Dir bc
	  Dm_vec = ublas::zero_vector<double> (cell_size);
	  ublas::vector<double> Gm (cell_size); // Dir bc
	  Gm = ublas::zero_vector<double> (cell_size);
	  ublas::vector<double> B (cell_size); // Neu bc 
	  B = ublas::zero_vector<double> (cell_size);
	  lowerboundary (geo, groundwater, active_lysimeter, h,
                         Kedge,
                         dq, Dm_mat, Dm_vec, Gm, B, msg);
	  upperboundary (geo, soil, T, surface, state, remaining_water, h,
                         Kedge,
                         dq, Dm_mat, Dm_vec, Gm, B, ddt, debug, msg, dt);
          Darcy (geo, Kedge, h, dq);

#ifdef TEST_OM_DEN_ER_BRUGT
          // update macropore flow components 
          for (int c = 0; c < cell_size; c++)
            {
              S_drain_sum[c] += S_drain[c] * ddt/dt;
              S_matrix_sum[c] += S_matrix[c] * ddt/dt;
            }
#endif

          std::vector<double> h_std_new (cell_size);
          std::copy(h.begin (), h.end (), h_std_new.begin ());

	  // Update remaining_water.
	  for (size_t i = 0; i < edge_above.size (); i++)
	    {
	      const int edge = edge_above[i];
	      const int cell = geo.edge_other (edge, Geometry::cell_above);
	      const double out_sign = (cell == geo.edge_from (edge))
		? 1.0 : -1.0;
	      remaining_water[i] += out_sign * dq (edge) * ddt;
              daisy_assert (std::isfinite (dq (edge)));
	    }

	  if (debug == 5)
	    {
	      std::ostringstream tmp;
	      tmp << "Remaining water at end: " << remaining_water;
	      msg.message (tmp.str ());
	    }

	  // Contribution to large time step.
          daisy_assert (std::isnormal (dt));
          daisy_assert (std::isnormal (ddt));
	  q += dq * ddt / dt;
          for (size_t e = 0; e < edge_size; e++)
            {
              daisy_assert (std::isfinite (dq (e)));
              daisy_assert (std::isfinite (q (e)));
            }
          for (size_t e = 0; e < edge_size; e++)
            {
              daisy_assert (std::isfinite (dq (e)));
              daisy_assert (std::isfinite (q (e)));
            }

	  time_left -= ddt;
	  iterations_with_this_time_step++;

	  if (iterations_with_this_time_step > time_step_reduction)
	    {
	      number_of_time_step_reductions--;
	      iterations_with_this_time_step = 0;
	      ddt *= time_step_reduction;
	    }
	}
      // End of small time step.
    }

  // Mass balance.
  // New = Old - S * dt + q_in * dt - q_out * dt + Error =>
  // 0 = Old - New - S * dt + q_in * dt - q_out * dt + Error
  Theta_error -= Theta;         // Old - New
  Theta_error -= S * dt;
#ifdef TEST_OM_DEN_ER_BRUGT
  for (size_t c = 0; c < cell_size; c++)
    Theta_error (c) -= (S_matrix_sum[c] + S_drain_sum[c]) * dt;
#endif
  
  for (size_t edge = 0; edge != edge_size; ++edge) 
    {
      const int from = geo.edge_from (edge);
      const int to = geo.edge_to (edge);
      const double flux = q (edge) * geo.edge_area (edge) * dt;
      if (geo.cell_is_internal (from))
        Theta_error (from) -= flux / geo.cell_volume (from);
      if (geo.cell_is_internal (to))
        Theta_error (to) += flux / geo.cell_volume (to);
    }

  // Find drain sink from mass balance.
#ifdef TEST_OM_DEN_ER_BRUGT
  std::fill(S_drain.begin (), S_drain.end (), 0.0);
#else
  std::vector<double> S_drain (cell_size);
#endif
  for (size_t i = 0; i < drain_cell.size (); i++)
    {
      const size_t cell = drain_cell[i];
      S_drain[cell] = Theta_error (cell) / dt;
      Theta_error (cell) -= S_drain[cell] * dt;
    }

  if (debug == 2)
    {
      double total_error = 0.0;
      double total_abs_error = 0.0;
      double max_error = 0.0;
      int max_cell = -1;
      for (size_t cell = 0; cell != cell_size; ++cell) 
        {
          const double volume = geo.cell_volume (cell);
          const double error = Theta_error (cell);
          total_error += volume * error;
          total_abs_error += std::fabs (volume * error);
          if (std::fabs (error) > std::fabs (max_error))
            {
              max_error = error;
              max_cell = cell;
            }
        }
      std::ostringstream tmp;
      tmp << "Total error = " << total_error << " [cm^3], abs = " 
	  << total_abs_error << " [cm^3], max = " << max_error << " [] in cell " 
	  << max_cell;
      msg.message (tmp.str ());
    }
  
  // Make it official.
  for (size_t cell = 0; cell != cell_size; ++cell) 
    soil_water.set_content (cell, h (cell), Theta (cell));
  
#ifdef TEST_OM_DEN_ER_BRUGT
  soil_water.add_tertiary_sink (S_matrix_sum);
  soil_water.drain (S_drain_sum, msg);
#endif


  for (size_t edge = 0; edge != edge_size; ++edge) 
    {
      daisy_assert (std::isfinite (q[edge]));
      soil_water.set_flux (edge, q[edge]);
    }

  soil_water.drain (S_drain, msg);

  // End of large time step.
}