/* fem_check21_tri.c  Galerkin FEM on triangular grid
 * solve 3 ux(x,y)  + 2 uy(x,y)  + u(x,y) = f(x,y)
 * boundary conditions computed using u(x,y)
 * analytic solution may be given by u(x) = 1+ 2*x + 3*y
 * f(x,y)=2.0*x+3.0*y+13.0
 * input triangles and coordinates
 * input geometry, root= 22_t  .coord,  .tri,  .bound
 * 
 * solve for Uij numerical approximation U(x_i,y_j) of u(x_i,y_j)
 * K * U = F, solve simultaneous equations for U
 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "simeq.h"
#include "phi_tri.h"
#include "phi_tri_cm.h"
#include "phi_tri_pts.h"

static int do_input();
static double area(double P1x, double P1y, /* area of triangle */
                   double P2x, double P2y,
                   double P3x, double P3y);
static int tri_int1(int np, double x1, double y1, double x2, double y2,
                    double x3, double y3, int debug);
static double tri_int2(int nv, double A[], double B[]);
static void uniqueint(int *n, int a[]);
static void sortint(int n, int a[]);
static void freevert(int *nc, int c[], int na, int a[], int nb, int b[]);
static int not_in(int i, int a[], int n);
static int reduce_boundary(void);

#undef  abs
#define abs(x) ((x)<0.0?(-(x)):(x))
#undef  min
#define min(a,b) ((a)<(b)?(a):(b))
#undef  max
#define max(a,b) ((a)>(b)?(a):(b))

/* maximum number of triangles or vertices, *3, squared */
#define sz 40
#define sz3 120
#define szsz 1600
#define szint 4000

static int    debug=3;       /* debug print level */
static char   root[]="22_tri"; /* root name for .tri, .coord, .bound */
static int    ntri;          /* number of triangles */ 
static int    minvert;       /* minimum vertex number, reduced to zero */
static int    t1[sz], t2[sz], t3[sz]; /* vertex numbers on triangles */
static int    ncoord;         /* number of coordinates, also equals nvert */
static double xc[sz], yc[sz]; /* coordinates in vertex order */
static int    b1[sz], b2[sz]; /* dirichlet boundary edge vertices */
static double xmin, xmax, ymin, ymax; /* determined by input */
static double kg[szsz];  /* global stiffness matrix */
static double fg[sz];    /* right hand side, forcing function */
static double ug[sz];    /* solution computed by fem */
static double Ua[sz];    /* known solution for checking */
static double cm1[6];    /* coefficients for phi functions */
static double cm2[6];    /* specific to second order two dimensions */
static double cm3[6];    /* for three vertices, in order */
static int uniquev[sz3]; /* all vertices */
static int nvert;        /* number of unique vertices */
static int uniqueb[sz3]; /* bound vertices */
static int nbound;       /* number of boundary vertices */
static int uniquef[sz3]; /* free vertices, 0 to nfree-1 */
static int nfree;        /* degrees of freedom  ntri-nbound */
static int nuniquev, nuniqueb, nuniquef; /* vertices counts */
static double xs[szint];   /* integration coordinates for this triangle */
static double ys[szint];
static double ph1[szint];   /* phi22(xs,ys) for this triangle cm1 */
static double ph2[szint];   /* phi22(xs,ys) for this triangle cm2 */
static double ph3[szint];   /* phi22(xs,ys) for this triangle cm3 */
static double gs1[szint];   /* galk(cm1,xs,ys) */
static double gs2[szint];   /* galk(cm2,xs,ys) */
static double gs3[szint];   /* galk(cm3,xs,ys) */
static double fs[szint];    /* F(xs,ys)        */

/* user provided, a specific differential equation */

double F(double x, double y) /* forcing function */
{
  return 2.0*x+3.0*y+13.0;
}

double uana(double x, double y) /* analytic solution and boundaries */
{
  return 1.0+2.0*x+3.0*y;
}

/* this is the encoding of the differential equation */
/* L(u(x,y))=F(x,y), L(u(x,y)) becomes Galerkin L(phi(x,y)) */
/* u=phi22  ux=phi22x  uy=phi22y uxx=phi22xx uxy=phi22xy uyy=phi22yy */
double galk(double c[], double x, double y)
{                         /* Galerkin k stiffness function for this problem */
  return  3.0 * phi12x (c,x,y) +
          2.0 * phi12y (c,x,y) +
                phi12  (c,x,y);
} /* end galk used for integration, c for phi_i, phi_j separate */



int main(int argc, char *argv[])
{
  double x, y, darea;
  double x1, y1, x2, y2, x3, y3;
  int    i1,     i2,     i3;
  double a, b, sum, intg;
  int np=30;   /* number of divisions for points to integrate */
  double c[6]; /* coefficients */
  double val, err, avgerr, maxerr;
  int stat, i, j, ii, k, kk, kdebug;
  int nv; /* number of vertices for numerical integration */
  int nuvert; /* number of unknown vertices after boundary applied */

  printf("fem_check21_tri.c running \n");
  printf("Given 3 ux + 2 uy + u = 2 x + 3 y + 13 \n");
  printf("Analytic solution u(x,y) =  1 + 2 x + 3 y \n");

  if(argc>1)
    strcpy(root,argv[1]);

  stat = do_input();
  if(stat != 0) return stat; /* can not continue */


  /* initialize kg, fg and ug  first cut, not using sparse matrix*/
  for(i=0; i<nvert; i++)
  {
    for(j=0; j<nvert; j++)
    {
      kg[i*nvert+j] = 0.0;
    }
  }

  for(i=0; i<nvert; i++)
  {
    fg[i] = 0.0; /* zero except at boundary */
    ug[i] = 0.0;
  }
  /* set boundary values (as used in fem_check21_tri.c) */
  for(i=0; i<nbound; i++)
  {
    j=uniqueb[i];
    fg[j]=uana(xc[j],yc[j]);
    kg[j*nvert+j]=1.0;
  }

  /* compute entries in stiffness matrix */
  printf("compute global stiffness matrix \n");
  for(k=0; k<ntri; k++) /* generate contribution for each triangle */
  {
    printf("computing local stiffness matrix for triangle %d \n", k);
    i1=t1[k];
    i2=t2[k];
    i3=t3[k];
    x1=xc[i1];
    y1=yc[i1];
    x2=xc[i2];
    y2=yc[i2];
    x3=xc[i3];
    y3=yc[i3];
    kdebug=debug;  /* (k<1?1:0); */
    darea=area(x1, y1, x2, y2, x3, y3);
    if(kdebug)
    {
      printf("nodes i1=%d, i2=%d, i3=%d, area=%f \n", i1, i2, i3, darea);
      printf("coord (%f,%f) (%f,%f) (%f,%f) \n",
             x1, y1, x2, y2, x3, y3);
    }
    /* determine coefficients for the three vertices */
    phi_tri_cm12(x1, y1, x2, y2, x3, y3, kdebug, cm1);
    phi_tri_cm12(x2, y2, x3, y3, x1, y1, kdebug, cm2);
    phi_tri_cm12(x3, y3, x1, y1, x2, y2, kdebug, cm3);
    /* build integration points and values*/
    nv = tri_int1(np, x1, y1, x2, y2, x3, y3, kdebug); 

    if(not_in(i1, uniqueb, nbound)) /* x1,y1 contributions if not boundary */
    {
      intg=tri_int2(nv, gs1, ph1)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i1, i1);
      kg[i1*nvert+i1] += intg;
      intg=tri_int2(nv, fs, ph1)*darea;
      if(kdebug) printf("intg fg =%f, at i=%d \n", intg, i1);
      fg[i1] += intg;
      intg=tri_int2(nv, gs2, ph1)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i2, i1);
      kg[i1*nvert+i2] += intg;
      /* if(not_in(i2, uniqueb, nbound)) kg[i2*nvert+i1] += intg; */
      intg=tri_int2(nv, gs3, ph1)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i3, i1);
      kg[i1*nvert+i3] += intg;
      /* if(not_in(i3, uniqueb, nbound)) kg[i3*nvert+i1] += intg; */
    }

    if(not_in(i2, uniqueb, nbound)) /* x2,y2 contributions if not boundary */
    {
      intg=tri_int2(nv, gs2, ph2)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i2, i2);
      kg[i2*nvert+i2] += intg;
      intg=tri_int2(nv, fs, ph2)*darea;
      if(kdebug) printf("intg fg =%f, at i=%d \n", intg, i2);
      fg[i2] += intg;
      intg=tri_int2(nv, gs1, ph2)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i1, i2);
      kg[i2*nvert+i1] += intg;
      /* if(not_in(i1, uniqueb, nbound)) kg[i1*nvert+i2] += intg; */
      intg=tri_int2(nv, gs3, ph2)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i3, i2);
      kg[i2*nvert+i3] += intg;
      /* if(not_in(i3, uniqueb, nbound)) kg[i3*nvert+i2] += intg; */
    }

    if(not_in(i3, uniqueb, nbound)) /* x3,y3 contributions if not boundary */
    {
      intg=tri_int2(nv, gs3, ph3)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i3, i3);
      kg[i3*nvert+i3] += intg;
      intg=tri_int2(nv, fs, ph3)*darea;
      if(kdebug) printf("intg fg =%f, at i=%d \n", intg, i3);
      fg[i3] += intg;
      intg=tri_int2(nv, gs1, ph3)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i1, i3);
      kg[i3*nvert+i1] += intg;
      /* if(not_in(i1, uniqueb, nbound)) kg[i1*nvert+i3] += intg; */
      intg=tri_int2(nv, gs2, ph3)*darea;
      if(kdebug) printf("integral=%f, at i=%d, j=%d \n", intg, i2, i3);
      kg[i3*nvert+i2] += intg;
      /* if(not_in(i2, uniqueb, nbound)) kg[i2*nvert+i3] += intg; */
    }
    printf("finished k=%d \n",k);
  } /* end k loop making local stifness matrices, inserted into global */ 

  if(debug>1) for(i=0; i<ntri; i++)
  {
    printf("tri %2d has vertices %2d  %2d  %2d \n",
                i, t1[i], t2[i], t3[i]);
  }

  if(debug>1) 
  {
    printf("vertices, coordinates and analytic values \n");
    for(i=0; i<nvert; i++)
    {
      Ua[i]=uana(xc[i],yc[i]);
      printf("coordinate %2d at  %5.2f , %5.2f, uana=%f \n",
	     i, xc[i], yc[i], Ua[i]);
    }
  }

  if(debug>2)
  {
    printf("k computed stiffness matrix, if debug \n");
    for(i=0; i<nvert; i++)
      for(j=0; j<nvert; j++)
        printf("K(%d,%d)=%f \n", i, j, kg[i*nvert+j]);
  }

  if(debug>2)
  {
    printf("f computed forcing function and boundary, if debug \n");
    for(i=0; i<nvert; i++)
      printf("F(%d)=%f, ug[%d]=%f \n", i, fg[i], i, ug[i]);
  }

  /* apply boundary conditions, must be largest vertices */
  nuvert=nvert;
  /* nuvert=reduce_boundary(); */

  simeq(nvert, kg, fg, ug); /* bad news, indexing, solve large matrix */

  avgerr = 0.0;
  maxerr = 0.0;
  printf("ug computed Galerkin, Ua analytic, error \n");
  for(i=0; i<nuvert; i++)
  {
    err = abs(ug[i]-Ua[i]);
    if(err>maxerr) maxerr = err;
    avgerr = avgerr + err;
    printf("ug[%d]=%6.3f, Ua=%6.3f, err=%g \n",
              i, ug[i], Ua[i], ug[i]-Ua[i]);
  }
  printf("                                        maxerr=%g, avgerr=%g \n",
         maxerr, avgerr/(double)(nvert));
  printf("\n");

  return 0;
} /* end main */


static int do_input()
{
  int i, j, ii;
  char fname[60];
  FILE * inp;
  int stat;

  /* read in triangles, set ntri */
  strcpy(fname, root);
  strcat(fname,".tri");
  if(debug>2) printf("about to read triangles from %s \n", fname);
  inp=fopen(fname, "r");
  if(inp==NULL)
  {
    printf("can not open %s for reading \n", fname);
    return 1;
  }
  stat=0;
  ntri=0;
  minvert=999999;
  nvert=0;
  if(debug>0) printf("triangles read from %s \n",fname);
  while(stat>=0)
  {
    stat=fscanf(inp, "%d %d %d", &t1[ntri], &t2[ntri], &t3[ntri]);
    if(stat<0) break;
    if(debug>0) printf("tri %2d has vertices %2d  %2d  %2d \n",
                ntri, t1[ntri], t2[ntri], t3[ntri]);
    if(t1[ntri]>nvert) nvert=t1[ntri];
    if(t2[ntri]>nvert) nvert=t2[ntri];
    if(t3[ntri]>nvert) nvert=t3[ntri];
    if(t1[ntri]<minvert) minvert=t1[ntri];
    if(t2[ntri]<minvert) minvert=t2[ntri];
    if(t3[ntri]<minvert) minvert=t3[ntri];
    ntri++;
  }
  fclose(inp);
  printf("subtracting minvert=%d from all vertices, using base zero\n",
         minvert);
  nvert=nvert+1-minvert; /* still must be dense sequence of numbers */
  nuniquev=0;
  for(i=0; i<ntri; i++)
  {
    t1[i]=t1[i]-minvert;
    t2[i]=t2[i]-minvert;
    t3[i]=t3[i]-minvert;
    uniquev[nuniquev]=t1[i];
    uniquev[nuniquev+1]=t2[i];
    uniquev[nuniquev+2]=t3[i];
    nuniquev=nuniquev+3;
  }
  uniqueint(&nuniquev, uniquev);

  /* read in dirichlet boundary, set nbound */
  strcpy(fname, root);
  strcat(fname,".bound");
  if(debug>2) printf("about to read boundary from %s \n", fname);
  inp=fopen(fname, "r");
  if(inp==NULL)
  {
    printf("can not open %s for reading \n", fname);
    return 1;
  }
  stat=0;
  nbound=0;
  if(debug>0) printf("Dirichlet boundaries from %s \n",fname);
  while(stat>=0)
  {
    stat=fscanf(inp, "%d %d", &b1[nbound], &b2[nbound]);
    if(stat<0) break;
    if(debug>0) printf("boundary segment %2d has vertices %2d  %2d \n",
                nbound, b1[nbound], b2[nbound]);
    if(b1[nbound]>nvert) printf("bad boundary vertex %d \n", b1[nbound]);
    if(b2[nbound]>nvert) printf("bad boundary vertex %d \n", b2[nbound]);
    if(b1[nbound]<minvert) printf("bad boundary vertex %d \n", b1[nbound]);
    if(b2[nbound]<minvert) printf("bad boundary vertex %d \n", b2[nbound]);
    nbound++;
  }
  fclose(inp);
  printf("subtracting minvert=%d from all vertices, using base zero\n", minvert);
  nuniqueb=0;
  for(i=0; i<nbound; i++)
  {
    b1[i]=b1[i]-minvert;
    b2[i]=b2[i]-minvert;
    uniqueb[nuniqueb]=b1[i];
    uniqueb[nuniqueb+1]=b2[i];
    nuniqueb=nuniqueb+2;
  }
  uniqueint(&nuniqueb,uniqueb);
  freevert(&nuniquef, uniquef, nuniquev, uniquev, nuniqueb, uniqueb);
  if(debug>0) printf("number of free vertices is %d \n", nuniquef);
  if(debug>0) for(i=0; i<nuniquef; i++) printf("free vertex %d \n", uniquef[i]);

  /* read in coordinates, check consistency */
  strcpy(fname, root);
  strcat(fname,".coord");
  if(debug>2) printf("about to read coordinates from %s \n", fname);
  inp=fopen(fname, "r");
  if(inp==NULL)
  {
    printf("can not open %s for reading \n", fname);
    return 1;
  }
  stat=0;
  ncoord=0;
  if(debug>0) printf("coordinates read from %s \n",fname);
  while(stat>=0)
  {
    stat=fscanf(inp, "%lf %lf", &xc[ncoord], &yc[ncoord]);
    if(stat<0) break;
    if(debug>0) printf("coordinate %2d at  %5.2f , %5.2f \n",
                       ncoord, xc[ncoord], yc[ncoord]);
    if(ncoord==0)
    {
      xmax=xc[ncoord];
      xmin=xc[ncoord];
      ymax=yc[ncoord];
      ymin=yc[ncoord];
    }
    if(xc[ncoord]>xmax) xmax=xc[ncoord];
    if(xc[ncoord]<xmin) xmin=xc[ncoord];
    if(yc[ncoord]>ymax) ymax=yc[ncoord];
    if(yc[ncoord]<ymin) ymin=yc[ncoord];
    ncoord++;
  }
  fclose(inp);
  if(ncoord!=nvert)
     printf("\n**** wrong number of coordinates=%d, should be %d\n",
	    ncoord, nvert);
  nfree=nvert-nbound;

  printf("xmin=%g, xmax=%g, ymin=%g, ymax=%g \n", xmin, xmax, ymin, ymax);
  printf("nvert=%d, nbound=%d, nfree=%d, ntri=%d \n",
	 nvert, nbound, nfree, ntri);
  return 0;
} /* end do_input */


static int tri_int1(int np, double x1, double y1, double x2, double y2,
                    double x3, double y3, int debug)
{
  int nv, i;

  nv=phi_tri_pts(np, x1, y1, x2, y2, x3, y3, xs, ys);
  if(debug) printf("tri_int1 running with np=%d, nv=%d \n",np,nv);
  for(i=0; i<nv; i++)
  {
    ph1[i]=phi12(cm1,xs[i],ys[i]);
    ph2[i]=phi12(cm2,xs[i],ys[i]);
    ph3[i]=phi12(cm3,xs[i],ys[i]);
    gs1[i]= galk(cm1,xs[i],ys[i]);
    gs2[i]= galk(cm2,xs[i],ys[i]);
    gs3[i]= galk(cm3,xs[i],ys[i]);
    fs[i]=F(xs[i],ys[i]);
  }
  return nv;
} /* end tri_int1 */

static double tri_int2(int nv, double A[], double B[])
{
  double sum, weight;
  double w1=1.0;
  double w2=2.0;
  double w3=3.0;
  int i;

  sum=w1*(A[0]*B[0]+A[1]*B[1]+A[2]*B[2]);
  weight=3.0*w1;
  sum=sum+w2*(A[3]*B[3]+A[4]*B[4]+A[5]*B[5]);
  weight=weight+3.0*w2;
  for(i=6; i<nv; i++)
  {
    sum=sum+w3*A[i]*B[i];
    weight=weight+w3;
  }
  return sum/weight;
}
static void uniqueint(int * n, int a[])
{
  int nold=*n;
  int i, j;
  if(debug>2) printf("finding unique of nold=%d \n", nold);
  sortint(nold,a);
  j=0;
  for(i=1; i<nold; i++)
  {
    if(a[i]>a[i-1])
    {
      j++;
      a[j]=a[i];
    }
  }
  j++;
  *n=j;
  if(debug>2) printf("found unique of n=%d \n", j);
} /* end uniqueint */

static void sortint(int n, int a[])
{
  int i, j, temp;
  for(i=0; i<n-1; i++)
    for(j=i+1; j<n; j++)
      if(a[i]>a[j]) {temp=a[i]; a[i]=a[j]; a[j]=temp;}
} /* end sortint */

static void freevert(int *nc, int c[], int na, int a[], int nb, int b[])
{
  /* a and b sorted integer arrays, c will be from a, not in b */
  int ia=0;
  int ib=0;
  int ic=0;
  if(debug>2) printf("freevert na=%d, nb=%d \n", na, nb);
  while(1)
  {
    if(ib>=nb || a[ia]!=b[ib]) {c[ic]=a[ia]; ic++; ia++;}
    else             {ia++; ib++;}
    if(ia>=na) break;
  }
  if(debug>2) printf("freevert nc free = %d \n", ic);
  *nc=ic;
}

static double area(double P1x, double P1y,
                   double P2x, double P2y,
                   double P3x, double P3y)
{
  double a;

  a = P1x*P2y - P2x*P1y + P2x*P3y - P3x*P2y + P3x*P1y - P1x*P3y;
  return abs(a)/2.0;
}

static int not_in(int i, int a[], int n)
{
  int found=0;
  int j;

  for(j=0; j<n; j++)
  {
    if(i==a[j]) { found=1; break; }
  }
  return 1-found;
} /* end not_in */

static int reduce_boundary(void)
{
  int i, j, k;
  int nuvert;

  nuvert=nvert;
  for(k=0; k<nbound; k++)
  {
    nuvert--;
    j=uniqueb[k];
    printf("applying boundary j=%d, at nvert-1=%d \n", j, nuvert);
    for(i=0; i<nvert; i++) /* could be to nuvert */
    {
      fg[i]=fg[i]-kg[i*nvert+nuvert]*ug[nuvert];
      kg[i*nvert+nuvert]=0.0; /* reduced */
    }
  }

  if(debug>1)
  {
    printf("f reduced forcing function with boundary, nuvert=%d \n",nuvert);
    for(i=0; i<nvert; i++)
      printf("F(%d)=%f, ug[%d]=%f \n", i, fg[i], i, ug[i]);
  }
  if(debug>1)
  {
    printf("k computed stiffness matrix, after boundary reduction \n");
    for(i=0; i<nvert; i++)
      for(j=0; j<nvert; j++)
        printf("K(%d,%d)=%f \n", i, j, kg[i*nvert+j]);
  }
  return nuvert;
} /* end reduce_boundary */

/* end fem_check21_tri.c */