// Emacs will be in -*- Mode: c++ -*-
//
// ********** DO NOT REMOVE THIS BANNER **********
//
// SUMMARY: Language for a Finite Element Method
// RELEASE: 2.0     
// USAGE  : You may copy freely these files and use it for    
//          teaching or research. These or part of these may   
//          not be sold or used for a commercial purpose with- 
//          out our consent : fax (33)1 44 27 44 11        
//
// AUTHORS:  D. Bernardi, Y. Darmaillac F. Hecht,    
//           P. Parole O. Pironneau C. Prud'homme
// ORG    :          
// E-MAIL :   pironneau@ann.jussieu.fr     
//
// ORIG-DATE:     June-94
// LAST-MOD:     16-Jan-96 at 16:10:15 by Prud'homme Christophe
//
// DESCRIPTION:  
// DESCRIP-END.
//


#define dwya(i,k) (-(q[me[k][next[i]]].x-q[me[k][next[i+1]]].x)/2)  /* area * ¦w^i/¦x|_T^k */
#define dwxa(i,k) ((q[me[k][next[i]]].y-q[me[k][next[i+1]]].y)/2) /* area * ¦w^i/¦y|_T^k */
#define dwy(i,k) (dwya(i,k)/area[k])  /* ¦w^i/¦x|_T^k */
#define dwx(i,k) (dwxa(i,k)/area[k])  /* area * ¦w^i/¦y|_T^k */

#define abss(a)(a < 0 ? -(a) : a)
#define mmax(a,b)(a>b?a:b)
#define mmin(a,b)(a<b?a:b)
#define ssqr(x) ((x)*(x))

#define penal (float)1.0e10
#define nhowmax 20

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/*
 * FreeFem includes
 */
#include <opclass.h>
#include <triangul.h>
#include <fem1.h>

extern int ns, nt;
extern rpoint *q;
extern triangle *me;
extern int *ng;
extern int *ngt;
extern float *area;
extern int next[5];
extern long bdth;
extern int rhsQuadra;

float
id (const float x)
{
  if (x != 0.)
    return 1.F;
  else
    return 0.F;
}
//float norm (const float x, const float y);  //{ return sqrt(sqr(x) + sqr(y));}

float
norme2 (const float &a)
{
  return a * a;
}

void
rhsPDE (int quadra, float *fw, float *f, float *g)
/* ------------------------------------------
   Computes the right hand side of the linear system of the Laplace equation
   $fw(j)=\int_\Omega f w^j + \int_{ng<>0} g w^j + penal|_{u0!=0} u0^j$ 
   if quadra = i then f is P^i, i=0 or 1
   OTHER INPUT next,area,q,me,ng,nt,penal
 */
{
  int j, k, k0, k1, k2, k3, ir, ir1, ir2, meirk, meirknext;
  float x1, x2;
  float aux;
  for (j = 0; j < ns; j++)
    fw[j] = 0.F;
  if (rhsQuadra)
    {
      rhsQuadra = 0;
      for (j = 0; j < ns; j++)
  fw[j] = f[j];
    }
  else
    for (k = 0; k < nt; k++)
      {

  for (ir = 0; ir <= 2; ir++)
    {
      k3 = 3 * k;
      ir1 = next[ir];
      ir2 = next[ir1];
      meirk = me[k][ir];
      meirknext = me[k][ir1];
      if (quadra)
        {
    k0 = k3 + ir;
    k1 = k3 + ir1;
    k2 = k3 + ir2;
        }
      else
        {
    k0 = me[k][ir];
    k1 = me[k][ir1];
    k2 = me[k][ir2];
        }
      x1 = 2 * f[k0];
      x1 += f[k1];
      x1 += f[k2];
      x1 *= area[k] / 12;
      fw[meirk] += x1;
    }
      }
  for (k = 0; k < nt; k++)
    for (ir = 0; ir <= 2; ir++)
      {
  k3 = 3 * k;
  ir1 = next[ir];
  ir2 = next[ir1];
  meirk = me[k][ir];
  meirknext = me[k][ir1];

  if ((ng[meirk] != 0) && (ng[meirknext] != 0))
    {
      if (quadra)
        {
    k0 = k3 + ir;
    k1 = k3 + ir1;
    k2 = k3 + ir2;
        }
      else
        {
    k0 = me[k][ir];
    k1 = me[k][ir1];
    k2 = me[k][ir2];
        }
      aux = norm (q[meirk].x - q[meirknext].x, q[meirk].y - q[meirknext].y) / 6;
      x1 = g[k0];
      x2 = g[k1];
      x1 *= aux;
      x2 *= aux;
      fw[meirk] += 2 * x1 + x2;
      fw[meirknext] += x1 + 2 * x2;
    }
      }
}


void
pdemat (int quadra, float *a, float *alpha,
  float *rho11, float *rho12, float *rho21, float *rho22,
  float *u1, float *u2, float *beta)
/* ------------------------------------------------ */
{
  long ai, k, i, j, ip, ipp, mejk, meik, k0, k1, k2, k3;
  long nsl = ns;
  float rhomean[2][2], alphamean, isii, x1, x2, x3, x4;
  long nsbdt = (2 * bdth + 1) * nsl;
  float aux;
  for (i = 0; i < nsbdt; i++)
    a[i] = 0.F;
  for (k = 0; k < nt; k++)
    for (i = 0; i <= 2; i++)
      {
  meik = me[k][i];
  ip = me[k][next[i]];
  ipp = me[k][next[i + 1]];
  if (quadra)
    {
      k3 = 3 * k;
      k0 = k3 + i;
      k1 = k3 + next[i];
      k2 = k3 + next[i + 1];
    }
  else
    {
      k0 = meik;
      k1 = ip;
      k2 = ipp;
    }
  x1 = rho11[k0];
  x2 = rho11[k1];
  x3 = rho11[k2];
  rhomean[0][0] = (x1 + x2 + x3) / 3;
  x1 = rho12[k0];
  x2 = rho12[k1];
  x3 = rho12[k2];
  rhomean[0][1] = (x1 + x2 + x3) / 3;
  x1 = rho21[k0];
  x2 = rho21[k1];
  x3 = rho21[k2];
  rhomean[1][0] = (x1 + x2 + x3) / 3;
  x1 = rho22[k0];
  x2 = rho22[k1];
  x3 = rho22[k2];
  rhomean[1][1] = (x1 + x2 + x3) / 3;
  x1 = alpha[k0];
  x2 = alpha[k1];
  x3 = alpha[k2];
  alphamean = (x1 + x2 + x3) / 3;
  for (j = 0; j <= 2; j++)
    {
      mejk = me[k][j];
      isii = i == j ? 1.F / 6.F : 1.F / 12.F;
      ai = nsl * (meik - mejk + bdth) + mejk;
      aux = dwxa (i, k) * dwx (j, k);
      x1 = rhomean[0][0] * aux;
      aux = dwya (i, k) * dwx (j, k);
      x2 = rhomean[1][0] * aux;
      aux = dwxa (i, k) * dwy (j, k);
      x3 = rhomean[0][1] * aux;
      aux = dwya (i, k) * dwy (j, k);
      x4 = rhomean[1][1] * aux;
      a[ai] += x1 + x2 + x3 + x4;
      x1 = u1[k0];
      x2 = u1[k1];
      x3 = u1[k2];
      a[ai] += (2 * x1 + x2 + x3) * dwxa (j, k) / 12;
      x1 = u2[k0];
      x2 = u2[k1];
      x3 = u2[k2];
      a[ai] += (2 * x1 + x2 + x3) * dwya (j, k) / 12
        + alphamean * area[k] * isii;
      if ((ng[meik] != 0) && (ng[mejk] != 0) && (meik < mejk))
        {
    if (quadra)
      {
        k0 = k3 + i;
        k1 = k3 + j;
      }
    else
      {
        k0 = meik;
        k1 = mejk;
      }
    x1 = beta[k0];
    x2 = beta[k1];
    x1 = (x1 + x2) * norm (q[meik].x - q[mejk].x, q[meik].y - q[mejk].y) / 2;
    a[ai] += x1 / 6;
    ai = nsl * bdth + mejk;
    a[ai] += x1 / 3;
    ai = nsl * bdth + meik;
    a[ai] += x1 / 3;
        }
    }
      }
}

/*----------------------------------------------------*/
float
gaussband (float *a, float *x, long n, long bdth, int first, float eps)
/*----------------------------------------------------*/
/* Factorise (first!=0) and/or solve    Ay = x  with result in x */
/* LU is stored in A ; returns the value of the smallest pivot  */
/*  all pivots less than eps are put to eps */
/*  a[i][j] is stored in a[i-j+bdth][j]=a[n*(i-j+bdth)+j) */
/*  where -bdwth <= i-j <= bdth */
{
  long i, j, k;
  float x1, x2, s, s1;
  float y1, s2;
  float smin = (float)1.0e9;

  if (first)      /* factorization */
    for (i = 0; i < n; i++)
      {
  for (j = mmax (i - bdth, 0); j <= i; j++)
    {
      s = 0.F;
      for (k = mmax (i - bdth, 0); k < j; k++)
        {
    x1 = a[n * (i - k + bdth) + k];
    x2 = a[n * (k - j + bdth) + j];
    s += x1 * x2;
        }
      a[n * (i - j + bdth) + j] -= s;
    }
  for (j = i + 1; j <= mmin (n - 1, i + bdth); j++)
    {
      s = 0.F;
      for (k = mmax (j - bdth, 0); k < i; k++)
        {
    x1 = a[n * (i - k + bdth) + k];
    x2 = a[n * (k - j + bdth) + j];
    s += x1 * x2;
        }
      s1 = a[n * bdth + i];
      smin = mmin (smin, norme2 (s1));
      if (smin < eps)
        s1 = id (1.F) * eps;
      x1 = a[n * (i - j + bdth) + j];
      a[n * (i - j + bdth) + j] = (x1 - s) / s1;
    }
      }
  for (i = 0; i < n; i++) /*  resolution */
    {
      s2 = 0.F;
      for (k = mmax (i - bdth, 0); k < i; k++)
  {
    x1 = a[n * (i - k + bdth) + k];
    y1 = x[k];
    s2 += x1 * y1;
  }
      y1 = x[i] - s2;
      x1 = a[n * bdth + i];
      x[i] = y1 / x1;
    }
  for (i = n - 1; i >= 0; i--)
    {
      s2 = 0.F;
      for (k = i + 1; k <= mmin (n - 1, i + bdth); k++)
  {
    x1 = a[n * (i - k + bdth) + k];
    y1 = x[k];
    s2 += x1 * y1;
  }
      x[i] -= s2;
    }
  return smin;
}
float
pdeian (float *a, float *u, float *f, float *g, float *u0,
  float *alpha, float *rho11, float *rho12, float *rho21, float *rho22,
  float *u1, float *u2, float *beta, int quadra, int factorize)
/*-----------
Solves  alpha u + (u1,u2)grad u- div(mat[rho] grad u) = f in ã, 
         u|_{ng<0} = u0, 
         beta u + mat[rho]¦u/¦n|_{ng<>0} = g
where f is P^quadra (quadra=0 or 1) 
*/
{
  long i, nsl = ns;
  int j, mekj, k, nquad = quadra ? 3 * nt : ns;
  if (factorize)
    pdemat (quadra, a, alpha, rho11, rho12, rho21, rho22, u1, u2, beta);
  rhsPDE (quadra, u, f, g);
  for (i = 0; i < nquad; i++)
    if (norme2 (u0[i]) != 0)
      {
  if (quadra)
    {
      k = i / 3;
      j = i - 3 * k;
      mekj = me[k][j];
    }
  else
    mekj = i;
  u[mekj] += u0[i] * penal;
  if (factorize)
    a[nsl * bdth + mekj] += id (u0[i]) * penal;
      }
  return gaussband (a, u, nsl, bdth, factorize, 1.F / penal);
}
