/******************************************************************************
 *
 * ALBERTA:  an Adaptive multi Level finite element toolbox  using
 *           Bisectioning refinement and Error control by Residual
 *           Techniques
 *
 * www.alberta-fem.de
 *
 ******************************************************************************
 *
 * File:     static-condensation.c
 *
 * Description: static condensation of a saddle point problem with 
 *              basis-functions: lagrange + bubble. 
 *              (especially for the mini element)
 *
 ******************************************************************************
 *
 *  author:     Rebecca Stotz
 *              Abteilung fuer Angewandte Mathematik
 *              Albert-Ludwigs-Universitaet Freiburg
 *              Hermann-Herder-Str. 10
 *              79104 Freiburg
 *              Germany
 *
 *  http://www.mathematik.uni-freiburg.de/IAM/ALBERTA
 *
 *  (c) by R. Stotz (2009)
 *
 *****************************************************************************/

/***************************************************************************** 
 *
 * The matrices A, B and Bt are all DOF_MATIX *-matrices. The static
 * condensation only works if 'A22' is diagonal. This is, for
 * instance, the case when finite element spaces underlying A22 is
 * composed from "bubble" functions which "live" only on one element.
 * 
 * The following saddle point problem
 *                                                          
 *         ( A11 | A12  |  B1 ) ( u1 )   ( f1 )                          
 *         ( A21 | A22  |  B2 )*( u2 ) = ( f2 )                          
 *         ( Bt1 | Bt2  |   0 ) (  p )   ( g  )                          
 *                                                                       
 * will be converted into:                     
 *                                                                       
 * fh_single = f1 - A12 * A22^(-1) * f2   = load_vector->dof_vec[0],      
 * gh_single = g - Bt2 * A22^(-1) * f2    = load_vector->dof_vec[1],      
 *                                                                       
 * A_single  = A11 - A12 * A22^(-1) * A21 = system_matrix->dof_mat[0][0], 
 * B_single  = B1 - A12 * A22^(-1) * B2   = system_matrix->dof_mat[0][1], 
 * Bt_single = (B_single)Transposed       = system_matrix->dof_mat[1][0], 
 * C_single  = - Bt2 * A22^(-1) * B2      = system_matrix->dof_mat[1][1], 
 *                                                                       
 *                                    u1  = up_h->dof_vec[0],             
 *                                    p   = up_h->dof_vec[1].      
 *       
 * New system (now there are no bubble-components):
 *                            
 *         ( A_single  | B_single ) (up_h->dof_vec[0])   (fh_single)     
 *         (                      )*(                ) = (         ) ,    
 *         ( Bt_single | C_single ) (up_h->dof_vec[1])   (gh_single)     
 *                                                                        
 * equivalent system:   
 *                        system_matrix * up_h = load_vector ,                 
 *                     
 *    
 *                                           
 * the bubble-component of u : 
 *
 *           u2 = A22^(-1)*(f2 - A21*u1 - B2*p),       
 *
 *   will be recalculated in the function 'expand_mini_spp()'.              
 *                                                                         
 ****************************************************************************/

#include <alberta/alberta.h>
#include <alberta/oem_block_solve.h>
#include "static-condensation.h"

void condense_mini_spp(const DOF_REAL_VEC_D *u_h, const DOF_REAL_VEC_D *f_h,
		       const DOF_REAL_VEC *g_h, BNDRY_FLAGS dirichlet_mask,
		       EL_MATRIX_INFO *A_minfo, EL_MATRIX_INFO *B_minfo,
		       BLOCK_DOF_MATRIX *system_matrix, BLOCK_DOF_VEC *up_h,
		       BLOCK_DOF_VEC *load_vector) 
{
  FUNCNAME("condense_mini_spp");
  
  int            i, j, k;
  REAL_D         tmp_d;
  REAL           tmp;
  FLAGS          fill_flag;
  EL_BNDRY_VEC *uh_bndry_bits 
    = get_el_bndry_vec(up_h->dof_vec[0]->fe_space->bas_fcts);
  EL_SCHAR_VEC *uh_bound 
    = get_el_schar_vec(up_h->dof_vec[0]->fe_space->bas_fcts);
  EL_DOF_VEC *uh_dof = get_el_dof_vec(up_h->dof_vec[0]->fe_space->bas_fcts);
  EL_DOF_VEC *ph_dof = get_el_dof_vec(up_h->dof_vec[1]->fe_space->bas_fcts);
  EL_REAL_D_VEC *fh_loc_single 
    = get_el_real_d_vec(up_h->dof_vec[0]->fe_space->bas_fcts);
  EL_REAL_VEC *gh_loc_single 
    = get_el_real_vec(up_h->dof_vec[1]->fe_space->bas_fcts);
  EL_MATRIX *Aloc_single = get_el_matrix(up_h->dof_vec[0]->fe_space, NULL, 
					 A_minfo->krn_blk_type);
  EL_MATRIX *Bloc_single = get_el_matrix(up_h->dof_vec[0]->fe_space, 
					 up_h->dof_vec[1]->fe_space,
					 B_minfo->krn_blk_type);
  EL_MATRIX *Cloc_single = get_el_matrix(up_h->dof_vec[0]->fe_space, NULL, 
					 MATENT_REAL);
  

  FOR_ALL_DOFS(up_h->dof_vec[0]->fe_space->admin,
	       COPY_DOW(((const REAL_D *)f_h->vec)[dof], 
			((REAL_D *)load_vector->dof_vec[0]->vec)[dof]);
	       COPY_DOW(((const REAL_D *)u_h->vec)[dof], 
			((REAL_D *)up_h->dof_vec[0]->vec)[dof]);
	       load_vector->dof_vec[1]->vec[dof] = g_h->vec[dof]);

  fill_flag = CALL_LEAF_EL|FILL_COORDS|FILL_BOUND;
  TRAVERSE_FIRST(u_h->fe_space->mesh, -1, fill_flag) {
    const EL_MATRIX      *Aloc, *Bloc;
    const EL_REAL_D_VEC  *fh_loc;
    const EL_MATRIX      *A11, *A12, *A21, *A22, *B1, *B2;
    const EL_REAL_D_VEC  *f1, *f2;
    
    Aloc = A_minfo->el_matrix_fct(el_info, A_minfo->fill_info);
    Bloc = B_minfo->el_matrix_fct(el_info, B_minfo->fill_info);
    
    fh_loc = fill_el_real_d_vec(NULL, el_info->el, (DOF_REAL_D_VEC *)f_h);
    
    get_dof_indices(uh_dof, up_h->dof_vec[0]->fe_space, el_info->el);
    get_dof_indices(ph_dof, up_h->dof_vec[1]->fe_space, el_info->el);
    
    get_bound(uh_bndry_bits, up_h->dof_vec[0]->fe_space->bas_fcts, el_info); 
    dirichlet_map(uh_bound, uh_bndry_bits, dirichlet_mask);
    
    f1 = fh_loc;
    f2 = CHAIN_NEXT(f1, EL_REAL_D_VEC);
    
    A11 = Aloc;
    A12 = ROW_CHAIN_NEXT(A11, EL_MATRIX);
    A21 = COL_CHAIN_NEXT(A11, EL_MATRIX);
    A22 = ROW_CHAIN_NEXT(A21, EL_MATRIX);
    
    B1 = Bloc;
    B2 = COL_CHAIN_NEXT(B1, EL_MATRIX);
    
    /* fh_loc_single = - A12 * A22^(-1) * f2 */
    for (i = 0; i < A12->n_row; i++) {
      AXEY_DOW(-A12->data.real[i][0]*1.0/A22->data.real[0][0], 
	       f2->vec[0], fh_loc_single->vec[i]);
    } 
    
    /* g_loc = - Bt2 * A22^(-1) * f2 */
    for (i = 0; i < B2->n_col; i++) {
      gh_loc_single->vec[i] = - 1.0/A22->data.real[0][0]
	*SCP_DOW(B2->data.real_d[0][i], f2->vec[0]);
    } 
    
    
    /* Aloc_single = A11 - A12 * A22^(-1) * A21 */
    for (i = 0; i < Aloc_single->n_row; i++) {
      for (j = 0; j < Aloc_single->n_col; j++) {
	tmp = 0.0;
	for (k = 0; k < A22->n_row; k++) {
	  tmp += A12->data.real[i][k] * 
	    1.0 / A22->data.real[k][k] * 
	    A21->data.real[k][j];
	}
	Aloc_single->data.real[i][j] = A11->data.real[i][j] - tmp;
      }
    }
    
    /* Bloc_single = B1 - A12 * A22^(-1) * B2 */
    for (i = 0; i < Bloc_single->n_row; i++) {
      for (j = 0; j < Bloc_single->n_col; j++) {
	SET_DOW(0.0, tmp_d);
	for (k = 0; k < A22->n_row; k++) {
	  AXPY_DOW(A12->data.real[i][k]*1.0/A22->data.real[k][k],
		   B2->data.real_d[k][j], tmp_d);
	}
	AXPBY_DOW(1.0, B1->data.real_d[i][j], -1.0, tmp_d, 
		  Bloc_single->data.real_d[i][j]);
      }
    }
    
    /* Cloc_single = - Bt2 * A22^(-1) * B2 */
    for (i = 0; i < Cloc_single->n_row; i++) {
      for (j = 0; j < Cloc_single->n_col; j++) {
	tmp = 0.0;
	SET_DOW(0.0, tmp_d);
	for (k = 0; k < A22->n_row; k++) {
	  AXEY_DOW(1.0 / A22->data.real[k][k], B2->data.real_d[k][j],
		   tmp_d);
	  tmp += SCP_DOW(tmp_d, B2->data.real_d[k][i]);
	}
	Cloc_single->data.real[i][j] = -tmp;
      }
    }
    /* load_vector->dof_vec[0] += fh_loc_single */
    add_element_d_vec((DOF_REAL_D_VEC *)load_vector->dof_vec[0], 1.0, 
		      fh_loc_single, uh_dof, uh_bound);

    /* load_vector->dof_vec[1] += gh_loc_single */
    add_element_vec((DOF_REAL_VEC *)load_vector->dof_vec[1], 1.0, 
		    gh_loc_single, ph_dof, NULL);
    
    add_element_matrix(system_matrix->dof_mat[0][0], A_minfo->factor,
		       Aloc_single, NoTranspose,
		       uh_dof, uh_dof, uh_bound);
    
    add_element_matrix(system_matrix->dof_mat[0][1], B_minfo->factor, 
		       Bloc_single, NoTranspose,
		       uh_dof, ph_dof, uh_bound);
    
    add_element_matrix(system_matrix->dof_mat[1][0], B_minfo->factor,
		       Bloc_single, Transpose,
		       ph_dof, uh_dof, NULL);
    
    add_element_matrix(system_matrix->dof_mat[1][1], 1.0, 
		       Cloc_single, NoTranspose, 
		       ph_dof, ph_dof, NULL);
    
  } TRAVERSE_NEXT();
  
  BNDRY_FLAGS_CPY(system_matrix->dof_mat[0][0]->dirichlet_bndry, 
		  dirichlet_mask);
   
  free_el_real_d_vec(fh_loc_single);
  free_el_real_vec(gh_loc_single);
  free_el_matrix(Aloc_single);
  free_el_matrix(Bloc_single);
  free_el_matrix(Cloc_single);
  
  free_el_schar_vec(uh_bound);
  free_el_bndry_vec(uh_bndry_bits);
  free_el_dof_vec(uh_dof);
  free_el_dof_vec(ph_dof);  
  
  
}

/***************************************************************************
 **** 'extended_spp_mini()' recomposes lagrange-basis functions and ********
 **** bubble basis fuctions                                 ****************
****************************************************************************/

void expand_mini_spp(const BLOCK_DOF_VEC *up_h, const DOF_REAL_VEC_D *f_h,
		     DOF_REAL_VEC_D *uh, BNDRY_FLAGS dirichlet_mask,
		     EL_MATRIX_INFO *A_minfo, EL_MATRIX_INFO *B_minfo)
{
  FUNCNAME("expand_mini_spp");

/************************************************************************* 
 * This function calculates the bubble-component of u : 
 *     
 *         u2 = A22^(-1)*(f2 - A21*u1 - B2*p)       
 *
 * and stores the calculated component and the 
 * lagrange-component 'up_h->dof_vec[0]' in the DOF_-vector 'uh'.
 * 
 * A22, A21, B2, u1 and f2 are the components of the saddelpoint problem:
 *         ( A11 | A12  |  B1 ) ( u1 )   ( f1 )                          
 *         ( A21 | A22  |  B2 )*( u2 ) = ( f2 )                          
 *         ( Bt1 | Bt2  |   0 ) (  p )   ( g  )            
 **************************************************************************/
  
  DOF_REAL_VEC_D *uh_bubble;
  FLAGS          fill_flag;
  int            j, k;

  uh_bubble = CHAIN_NEXT(uh, DOF_REAL_VEC_D);
  dof_set_dow(0.0, uh_bubble);
  dof_set_dow(0.0, uh);
  FOR_ALL_DOFS(f_h->fe_space->admin,
	       COPY_DOW(((REAL_D *)up_h->dof_vec[0]->vec)[dof],
			((REAL_D *)uh->vec)[dof]));
  
  
  fill_flag = CALL_LEAF_EL|FILL_COORDS|FILL_BOUND;
  TRAVERSE_FIRST(f_h->fe_space->mesh, -1, fill_flag) {
    const EL_REAL_VEC_D *u1;
    const EL_REAL_D_VEC *f1, *f2;
    const EL_MATRIX     *A11, *B1;
    const EL_MATRIX     *A21, *A22;
    const EL_MATRIX     *B2;
    const EL_REAL_VEC   *ploc;
    REAL_D              B2p;
    DOF                 bubble_dof;
    REAL_D              bubble_coeff;    
   
    SET_DOW(0.0, B2p);
    SET_DOW(0.0, bubble_coeff);

    u1 = fill_el_real_vec_d(NULL, el_info->el, up_h->dof_vec[0]);
    f1 = fill_el_real_d_vec(NULL, el_info->el, (DOF_REAL_D_VEC *)f_h);
    ploc = fill_el_real_vec(NULL, el_info->el, 
			    (DOF_REAL_VEC *)up_h->dof_vec[1]);
    
    f2 = CHAIN_NEXT(f1, EL_REAL_D_VEC);
    
    A11 = A_minfo->el_matrix_fct(el_info, A_minfo->fill_info);
    B1 = B_minfo->el_matrix_fct(el_info, B_minfo->fill_info);

    A21 = COL_CHAIN_NEXT(A11, EL_MATRIX);
    A22 = ROW_CHAIN_NEXT(A21, EL_MATRIX);    
    B2 = COL_CHAIN_NEXT(B1, EL_MATRIX);

    GET_DOF_INDICES(uh_bubble->fe_space->bas_fcts, el_info->el,
		    uh_bubble->fe_space->admin, &bubble_dof);

    
    /* u2 = A22^(-1)*(f2 - A21*u1 - B2*p) */
    for (j = 0; j < A21->n_col; j++) {
      AXPY_DOW(-A21->data.real[0][j], ((const REAL_D *)u1->vec)[j], 
	       bubble_coeff);
    }
    for (k = 0; k < B2->n_col; k++) {
      AXPY_DOW(ploc->vec[k], B2->data.real_d[0][k], B2p);
    }
    AXPBYP_DOW(1.0, f2->vec[0], -1.0, B2p, bubble_coeff);
    SCAL_DOW(1./ A22->data.real[0][0], bubble_coeff);
    
    AXPY_DOW(1.0, bubble_coeff, ((REAL_D *)uh_bubble->vec)[bubble_dof]);

  } TRAVERSE_NEXT();

}
