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

#define SimpleSpring_C_author \
  "Modified by L.A.P.Lozada on 2000-11-18."

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

TYPE
  bool_vec_t == ARRAY OF bool_t;
  double_vec_t == double_vec_t;

REVEAL
  T == Public BRANDED OBJECT
      double K;               /* The energy normalization factor */    
      ElemTableRec_t *top;     /* The map elements. */
      termVar: REF bool_vec_t;        /* TRUE if node is variable & existing */
      @{edge->?}Relevant: REF bool_vec_t;   /* TRUE if @{edge->?} is relevant */
      eDdif: REF double_vec_t           /* (Work) Gradient of "e" rel. to "dif" */
    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->K = 1.0;
    erg->top = top;
    erg->termVar = bool_vec_new(top->node.nel);
    erg->edgeRelevant = bool_vec_new(top->NE);
    erg->eDdif = double_vec_new(top->NE);
      
    /* Just in case the client forgets to call "defVar": */
    for (i = 0; i < top->node.nel; i++){ erg->termVar^[i] = FALSE; }
    for (j = 0; j < top->NE; j++){ erg->edgeRelevant^[j] = FALSE; }
  } /* END DefTop */
 
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /*
      Decide which @{edge->?}s are relevant to spring energy. A @{edge->?} is relevant
      iff it has at least one endpoint node that is variable.
    */
    ??? NV = erg->top->node.nel;
    ??? NE = erg->top->NE;
    ??? termVar = erg->termVar^;
    ??? @{edge->?} = erg->top->edge^;
    ??? @{edge->?}Relevant = erg->edgeRelevant^;
    {
      assert((variable.nel) == NV);
      for (v = 0; v < NV; v++)
      {
        termVar[v] = variable[v];
      }
 
      /* Find the relevant @{edge->?}s : */
      for (k = 0; k < NE; k++){ @{edge->?}Relevant[k] = FALSE; }
      for (k = 0; k < NE; k++)
      {
        ??? e = @{edge->?}[k];
        ??? u = NARROW(OrgV(e.pa), Triangulation.Node);
        ??? v = NARROW(OrgV(Clock(e.pa)), Triangulation.Node);         
        ??? vvar = (termVar[u->num]) || (termVar[v->num]);
        {
          if (vvar)
          {
            @{edge->?}Relevant[k] = TRUE;
          }
        }
      }
    }
  } /* END DefVar */

void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { int NV = erg->top->node.nel;
    ??? NE = erg->top->NE;
    ??? @{edge->?} = erg->top->edge^;
    ??? @{edge->?}Relevant = erg->edgeRelevant^;
    ??? K = erg->K;
    ??? L = ((double)erg->length);
    ??? eDdif = erg->eDdif^;
    ??? termVar = erg->termVar;
    {
      void AccumTerm(*cu,cv: r4_t; VAR eDdif : double)
        /*
          Adds to "e" the energy term corresponding to a nodes "cv"
          and "cu". Returns also the gradient "eDdif".
        */
        CONST Epsilon == 1.0d-10; 
        {
          ??? n = r4_Sub(cu,cv);
          ??? dif = r4_Norm(n);
          ??? d2  = dif * dif + Epsilon;
          ??? l2  = L * L     + Epsilon;
          ??? d3  = d2 * dif  + Epsilon;
          {
            e = e + K * ((d2/l2) + (l2/d2) - 2.0);
            if (grad)
            {
              eDdif = 2.0 * K * ((dif/l2)-(l2/d3));
            }
            else
            {
              eDdif = 0.0;
            }
          }
       } AccumTerm;


      void Distribute_eDdif(*u,v: uint;
                                 *eDdif: double)
        /* Distribute eDdif on endpoints "c[u]" and "c[v]" */
        CONST Epsilon == 1.0d-10;
        {
          ??? cu = c[u];
          ??? cv = c[v];
          ??? n = r4_Sub(cu,cv);
          ??? dif = r4_Norm(n)+Epsilon;
          ??? eDv = eDc[v];
          ??? eDu = eDc[u];
          {
            ??? difDcu = r4_Scale(1.0/dif, n);
            ??? difDcv = r4_Scale(-1.0/dif,n);
            ??? eDcu   = r4_Scale(eDdif, difDcu);
            ??? eDcv   = r4_Scale(eDdif, difDcv);
            {
              if (termVar[v])
              {
                eDv = r4_Add(eDv, eDcv);
              }
              if (termVar[u])
              {
                eDu = r4_Add(eDu, eDcu);
              }
            }
          }
        } Distribute_eDdif;

      {
        for (i = 0; i < NV; i++){ eDc[i]=(r4_t){0.0, 0.0, 0.0, 0.0}; }
        e = 0.0;
        for (k = 0; k < NE; k++)
        {
          ??? e = @{edge->?}[k];
          Node_t un = OrgV(e.pa)->num;
          Node_t vn = OrgV(Clock(e.pa))->num;
          {
            if (@{edge->?}Relevant[k])
            {
              AccumTerm(c[un], c[vn], eDdif[k]);
              if (grad)
              {
                Distribute_eDdif(un, vn, eDdif[k]);
              }
            }
          }
        }
      }
    }
  } /* END Eval */


char * SELF_Name(SELF_T *erg)==
  { return
      "SimpleSpring(iLen = " & Fmt.Real(erg->length,Fmt.Style.Fix,prec = 3) \
      ")"
  } /* END Name */

{
} SimpleSpring.