/* See {ProxTetraEnergy.h} */
#include <ProxTetraEnergy.h>

#define ProxTetraEnergy_C_author \
  "Modified by L.A.P.Lozada on 99-08-19."

#include <Triangulation.h>
#include <Energy.h>
#include <r4.h>
#include <math.h>

   /* [[!!! DOES THIS COMMENT FROM Octf.h APPLY HERE ??? */
   /* A '@{chip->?}' is a group of four @places, consisting of two mutually dual
   wedges. */

TYPE 
  FourNodes_t == RECORD u, v, w, x: uint; }
  bool_vec_t == ARRAY OF bool_t;
  double_vec_t == double_vec_t;

REVEAL
  T == Public BRANDED OBJECT
    ElemTableRec_t *top;
    uint nT;            /* Number of tetrahedrons existing on "triang" */
    double K;             /* Energy normalization factor */      
    bool_vec_t vVar;         /* Which nodes are variable */
    quad: REF ARRAY OF FourNodes_t; /* The existing tetrahedrons */
    pVar: REF bool_vec_t;         /* Which tetrahedrons in "@{chip->?}" are variable */
    bar: REF Coords_t;         /* (Work) Barycenters of tetrahedrons in "@{chip->?}" */
    eDbar: REF Coords_t;       /* (Work) Gradient of "e" rel "bar" */
    rsq: REF double_vec_t;          /* (Work) Nominal tetrahedron radius, squared */
    eDrsq: REF double_vec_t;        /* (Work) Gradient of "e" rel "rsq" */
   OVERRIDES
     init = Init;
     defTop = DefTop;
     defVar = DefVar;
     eval = Eval; 
     name = Name;
   }

SELF_Init(SELF_T *erg)
  { return erg;
  } /* END Init */

void SELF_DefTop(SELF_T *erg, ElemTableRec_t *top)
  { erg->top = top;
    /* Work areas: */
    erg->quad = CollectExistingTetrahedrons(top);
    erg->NT = ((erg->quad^).nel);
    erg->K = 1.0/((double)erg->NT*erg->NT);
    erg->vVar = bool_vec_new(top->node.nel);
    erg->pVar = bool_vec_new(erg->NT);
    erg->bar = NEW(REF Coords_t, erg->NT);
    erg->eDbar = NEW(REF Coords_t, erg->NT);
    erg->rsq = double_vec_new(erg->NT);
    erg->eDrsq = double_vec_new(erg->NT);
    /* In case the client forgets to call "defVar": */
    for (i = 0; i < erg->NT; i++){ erg->pVar[i] = TRUE; }
  } /* END DefTop */
  
PROCEDURE CollectExistingTetrahedrons(ElemTableRec_t *top): REF ARRAY OF FourNodes_t ==
      NT: uint = 0;
  { ??? NP = top->cell.nel;
    ??? t = NEW(REF ARRAY OF FourNodes_t, NP)^;
    {
      for (i = 0; i < NP; i++)
      {
        ??? p = top->cell[i];
        ??? a = Srot(top.cell[i]);
        {
          if ((p->exists) && (DegreeOfNode(a) == 4))
          {
            ??? u = p.node[0]; ??? un = NARROW(u, Triangulation.Node);
            ??? v = p.node[1]; ??? vn = NARROW(v, Triangulation.Node);
            ??? w = p.node[2]; ??? wn = NARROW(w, Triangulation.Node);
            ??? x = p.node[3]; ??? xn = NARROW(x, Triangulation.Node);
            {
              assert(un->exists) && (vn->exists) && (wn->exists) && (xn->exists);
              t[NT] = FourNodes_t{u->num, v->num, w->num, x->num}
            }
            NT++
          }
        }
      }
      ??? r = NEW(REF ARRAY OF FourNodes_t, NT);
    {
        r^ = SUBARRAY(t, 0, NT);
        return r;
      }
    }
  } /* END CollectExistingTetrahedrons */

void SELF_DefVar(SELF_T *erg, *variable: ARRAY OF bool_t)
  { /*
      Separate the existing tetrahedrons now in "triang"
      into the variable list "pVar" and the fixed list 
      "tfix".  (A tetrhedron is variable if any of its nodes
      is variable.)
    */
    ??? NT = erg->NT;
    int NV = erg->top->node.nel;
    bool_vec_t vVar = erg->vVar^;
    ??? quad = erg->quad^;
    ??? pVar = erg->pVar^;
    {
      assert((variable.nel) == NV);
      vVar = variable;

      for (i = 0; i < NT; i++)
      {
        ??? ti = quad[i];
    {
          pVar[i] = vVar.el[ti.u]) || (vVar.el[ti.v]) || (vVar.el[ti.w]) || (vVar.el[ti.x]
        }
      }
    }
  } /* END DefVar */

void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { ??? NT = erg->NT;
    int NV = erg->top->node.nel;
    bool_vec_t vVar = erg->vVar^;
    ??? quad = erg->quad^;
    ??? pVar = erg->pVar^;
    ??? bar = erg->bar^;
    ??? eDbar = erg->eDbar^;
    ??? rsq = erg->rsq^;
    ??? eDrsq = erg->eDrsq^; 
    ??? fuzz = ((double)erg->fuzz);
    ??? fuzz2 = fuzz*fuzz;
    ??? K = erg->K;
    {
    
      double Compute_rsq(*t: FourNodes_t)
        /*
          Computes the 'radius squared' of "t", actually
          the mean squared node-barycenter distance
        */
        double r = 0.0;
        VAR {
          ??? u = c[t.u];
          ??? v = c[t.v];
          ??? w = c[t.w];
          ??? x = c[t.x];
          {
            ??? uv0 = u[0] - v[0], uw0 = u[0] - w[0], ux0 = u[0] - x[0];
            ??? vw0 = v[0] - w[0], vx0 = v[0] - x[0], wx0 = w[0] - x[0];
            {
              r = r + uv0*uv0 + uw0*uw0 + ux0*ux0 + vw0*vw0 + vx0*vx0 + wx0*wx0
            }
            ??? uv1 = u[1] - v[1], uw1 = u[1] - w[1], ux1 = u[1] - x[1];
            ??? vw1 = v[1] - w[1], vx1 = v[1] - x[1], wx1 = w[1] - x[1];
            {
              r = r + uv1*uv1 + uw1*uw1 + ux1*ux1 + vw1*vw1 + vx1*vx1 + wx1*wx1
            }
            ??? uv2 = u[2] - v[2], uw2 = u[2] - w[2], ux2 = u[2] - x[2];
            ??? vw2 = v[2] - w[2], vx2 = v[2] - x[2], wx2 = w[2] - x[2];
            {
              r = r + uv2*uv2 + uw2*uw2 + ux2*ux2 + vw2*vw2 + vx2*vx2 + wx2*wx2
            }
             ??? uv3 = u[3] - v[3], uw3 = u[3] - w[3], ux3 = u[3] - x[3];
             ??? vw3 = v[3] - w[3], vx3 = v[3] - x[3], wx3 = w[3] - x[3];
             {
              r = r + uv3*uv3 + uw3*uw3 + ux3*ux3 + vw3*vw3 + vx3*vx3 + wx3*wx3
            }
            return r/16.0
          }
        } Compute_rsq;
        
      void Distribute_eDrsq(*t: FourNodes_t; *eDr2: double)
        /*
          Adds to the gradient "eDc" the component due to the influence
          "eDr2" of the radius squared of "quad" on the energy.
        */
        {
          ??? eDs = 2.0/16.0*eDr2;
          ??? u = c[t.u], v = c[t.v], w = c[t.w], x = c[t.x]
          {
            if (vVar.el[t.u])
            {
              ??? eDu = eDc[t.u];
    {
                eDu[0] = eDu[0] + eDs * (u[0]-v[0] + u[0]-w[0] + u[0]-x[0]);
                eDu[1] = eDu[1] + eDs * (u[1]-v[1] + u[1]-w[1] + u[1]-x[1]);
                eDu[2] = eDu[2] + eDs * (u[2]-v[2] + u[2]-w[2] + u[2]-x[2]);
                eDu[3] = eDu[3] + eDs * (u[3]-v[3] + u[3]-w[3] + u[3]-x[3]);
              }
            }

            if (vVar.el[t.v])
            {
              ??? eDv = eDc[t.v];
    {
                eDv[0] = eDv[0] + eDs * (v[0]-u[0] + v[0]-w[0] + v[0]-x[0]);
                eDv[1] = eDv[1] + eDs * (v[1]-u[1] + v[1]-w[1] + v[1]-x[1]);
                eDv[2] = eDv[2] + eDs * (v[2]-u[2] + v[2]-w[2] + v[2]-x[2]);
                eDv[3] = eDv[3] + eDs * (v[3]-u[3] + v[3]-w[3] + v[3]-x[3]);
              }
            }

            if (vVar.el[t.w])
            {
              ??? eDw = eDc[t.w];
    {
                eDw[0] = eDw[0] + eDs * (w[0]-v[0] + w[0]-u[0] + w[0]-x[0]);
                eDw[1] = eDw[1] + eDs * (w[1]-v[1] + w[1]-u[1] + w[1]-x[1]);
                eDw[2] = eDw[2] + eDs * (w[2]-v[2] + w[2]-u[2] + w[2]-x[2]);
                eDw[3] = eDw[3] + eDs * (w[3]-v[3] + w[3]-u[3] + w[3]-x[3]);
              }
            }

           if (vVar.el[t.x])
           {
              ??? eDx = eDc[t.x];
    {
                eDx[0] = eDx[0] + eDs * (x[0]-v[0] + x[0]-w[0] + x[0]-u[0]);
                eDx[1] = eDx[1] + eDs * (x[1]-v[1] + x[1]-w[1] + x[1]-u[1]);
                eDx[2] = eDx[2] + eDs * (x[2]-v[2] + x[2]-w[2] + x[2]-u[2]);
                eDx[3] = eDx[3] + eDs * (x[3]-v[3] + x[3]-w[3] + x[3]-u[3]);
              }
            } 
            
          }
        } Distribute_eDrsq;
          
      r4_t Compute_bar(*t: FourNodes_t)
        r4_t b;
        VAR {
          ??? u = c[t.u];
            ??? v = c[t.v];
          ??? w = c[t.w];
          ??? x = c[t.x];
          {
            b[0]= (u[0] + v[0] + w[0] + x[0])/4.0;
            b[1]= (u[1] + v[1] + w[1] + x[1])/4.0;
            b[2]= (u[2] + v[2] + w[2] + x[2])/4.0;
            b[3]= (u[3] + v[3] + w[3] + x[3])/4.0;
            return b
          }
        } Compute_bar;
      
      void Distribute_eDbar(*t: FourNodes_t; *eDb: r4_t)
        /*
          Adds to the gradient "eDc" the component due to the influence
          "eDb" of "quad"'s barycenter on the energy.
        */
        {
          ??? eDs0 = eDb[0]/4.0;
            ??? eDs1 = eDb[1]/4.0;
          ??? eDs2 = eDb[2]/4.0;
          ??? eDs3 = eDb[3]/4.0;
          {
            if (vVar.el[t.u])
            {
              ??? eDu = eDc[t.u];
    {
                eDu[0] = eDu[0] + eDs0;
                eDu[1] = eDu[1] + eDs1;
                eDu[2] = eDu[2] + eDs2;
                eDu[3] = eDu[3] + eDs3;
              }
            }

            if (vVar.el[t.v])
            {
              ??? eDv = eDc[t.v];
    {
                eDv[0] = eDv[0] + eDs0;
                eDv[1] = eDv[1] + eDs1;
                eDv[2] = eDv[2] + eDs2;
                eDv[3] = eDv[3] + eDs3;
              }
            }

            if (vVar.el[t.w])
            {
              ??? eDw = eDc[t.w];
    {
                eDw[0] = eDw[0] + eDs0;
                eDw[1] = eDw[1] + eDs1;
                eDw[2] = eDw[2] + eDs2;
                eDw[3] = eDw[3] + eDs3;
              }
            }

            if (vVar.el[t.w])
            {
              ??? eDw = eDc[t.w];
    {
                eDw[0] = eDw[0] + eDs0;
                eDw[1] = eDw[1] + eDs1;
                eDw[2] = eDw[2] + eDs2;
                eDw[3] = eDw[3] + eDs3;
              }
            }

            if (vVar.el[t.x])
            {
              ??? eDx = eDc[t.x];
    {
                eDx[0] = eDx[0] + eDs0;
                eDx[1] = eDx[1] + eDs1;
                eDx[2] = eDx[2] + eDs2;
                eDx[3] = eDx[3] + eDs3;
              }
            }
          }
        } Distribute_eDbar;

      void AddTetrahedronPlace_tEnergy(i, j: uint)
        /*
          Adds to "e" the electric potential energy between the 
          tetrahedrons "quad[i]" and "quad[j]", from "bar[i],
          "bar[j]", "rsq[i]", and "rsq[j]". Assumes "i!=j".

          Also adds to "eDrsq[i]", "eDrsq[j]", "eDbar[i]"
          and "eDbar[j]" the corresponding derivatives,
          if the tetrahedrons are variable.
        */
        {
          ??? bi = bar[i],     bj = bar[j];
          ??? eDbi = eDbar[i],  eDbj = eDbar[j];
          ??? si = rsq[i],      sj = rsq[j];
          ??? eDsi = eDrsq[i],  eDsj = eDrsq[j];
           
          ??? vij = r4_Sub(bj, bi);
          ??? s = si + sj;
          ??? r2 = r4_NormSqr(vij) + fuzz2 * s;
          ??? r = sqrt(r2);
          ??? a = K/r;
          {
            e = e + a;
            if (grad)
            {
              ??? aDr = - a/r;
              ??? aDr2 = aDr * 0.5 / r;
              ??? aDs = aDr2 * fuzz2;
              ??? aDvij = r4_Scale(2.0 * aDr2, vij);
              {
                if (pVar[i])
                {
                  eDsi = eDsi + aDs;
                  eDbi = r4_Sub(eDbi, aDvij)
                }
                if (pVar[j])
                {
                  eDsj = eDsj + aDs;
                  eDbj = r4_Add(eDbj, aDvij)
                }
              }
            }
          }
        } AddTetrahedronPlace_tEnergy;

      {
        /* Compute barycenters and 'radius squared' of quad[i], clear derivatives: */
        for (i = 0; i < NT; i++)
        {
          bar[i] = Compute_bar(quad[i]);
          rsq[i] = Compute_rsq(quad[i]);
          if (grad)
          {
            eDbar[i] = (r4_t){0.0, 0.0, 0.0, 0.0};
            eDrsq[i] = 0.0;
          }
        }
        
        /* Compute energies and the internal derivatives "eDrsq", "eDbar": */
        e = 0.0;
        for (i = 0; i < NT; i++)
        {
          if (pVar[i])
          {
            for (j = 0; j < NT; j++)
            {
              if ((i!=j) && ((NOT pVar[j]) || (i < j)))
              {
                AddTetrahedronPlace_tEnergy(i, j)
              }
            }
          }
        }

        /* Distribute "eDrsq", "eDbar" onto "eDc": */
        for (i = 0; i < NV; i++){ eDc[i] = (r4_t){0.0, 0.0, 0.0, 0.0}; }
        if (grad)
        {
          for (i = 0; i < NT; i++)
          {
            if (pVar[i])
            {
              Distribute_eDbar(quad[i], eDbar[i]);
              Distribute_eDrsq(quad[i], eDrsq[i]);
            }
          }
        }
      }
    }
  } /* END Eval */

char * SELF_Name(SELF_T *erg)==
  { return "Elect(fuzz = " & Fmt.Real(erg->fuzz, prec = 3) & ")";
  } /* END Name */

{
} ProxTetraEnergy.