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

/* In this module non exists an Apropiate factor normalization for this energy,
   actually it is 0.5.

   Revisions:

   18-11-2000:  Added the constant value Epsilon to avoid possiblev infinite
                values (Nan).
*/

#define _GNU_SOURCE
#include <stdio.h>
#include <math.h>
#include <Triangulation.h>
#include <r4.h>
#include <Energy.h>

CONST Epsilon == 1.0d-10;

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

double inf = Math.pow(10.0,10.0);
  VAR 
REVEAL
  T == Public BRANDED OBJECT
      double F;               /* The energy normalization factor */    
      ElemTableRec_t *top;     /* The map elements. */
      termVar: REF bool_vec_t;        /* TRUE if node is variable & existing */
      m:  REF AdjMatrix;         /* Matrix of initial distances */
      eDdif: REF double_vec_t_vec_t; /* (Work) Gradient of "e" rel. to "dif"  */
      double L;              /* Kamada' constant */
      l,k: REF AdjMatrix;        /* parameters: lij is the original length of
                                    the spring between pi and pj; kij is the
                                    strength of  the spring  pi and pj. */
                                 
    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)
  double *dmax;
  { ??? NV = top->node.nel;
    ??? length = ((double)erg->length);
    ??? strength = ((double)erg->strength);
    {
      erg->m = MakeAdjMatrix(top);
      ShortestPath(erg->m, NV);
      if (erg->detail)
      {
        PrintHalfMatrix(erg->m, NV);
      } 
      dmax    = FindMaxDistance(erg->m, NV);
      erg->L   = length/dmax;
      erg->F   = 0.50;  
      erg->top = top;
      erg->termVar = bool_vec_new(NV);
      erg->eDdif   = double_vec_t_vec_new(NV, NV);
      erg->l = CalculateLength (erg->m, NV, erg->L);
      erg->k = CalculateStrengh(erg->m, NV, strength);
      if (erg->detail)
      {
        PrintHalfMatrix(erg->k, NV);
      }
      /* Just in case the client forgets to call "defVar": */
      for (i = 0; i < top->node.nel; i++){ erg->termVar^[i] = FALSE; }
    }
  } /* END DefTop */
 
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /* Decides which nodes are relevant to kamada energy. A node is
       relevant iff it is variable.
    */
    ??? NV = erg->top->node.nel;
    ??? termVar = erg->termVar^;
    {
      /* Find the relevant nodes: */
      assert((variable.nel) == NV);
      for (v = 0; v < NV; v++)
      {
        termVar[v] = variable[v];
      }
    }
  } /* END DefVar */

AdjMatrix *MakeAdjMatrix(ElemTableRec_t *top, 
 )==
  REF ARRAY OF double_vec_t m ;
    v: REF ARRAY OF INTEGER;
  { m = NEW(REF ARRAY OF double_vec_t, top->node.nel, top->node.nel);
    for (i = 0; i < top->node.nel; i++)
    {
      for (j = 0; j < top->node.nel; j++)
      {
        m[i,j] = inf;
      }
    }

    for (i = 0; i < top->node.nel; i++)
    {
      ??? a = top->node[i];
      ??? star = Triangulation.Neighbors(OrgV(a),top);
      ??? nv = ((star^).nel);
      {
        m[i,i] = 0.0;
        v = NEW(REF ARRAY OF INTEGER, nv);
        for (k = 0; k < nv; k++)
        {
          v[k] = star[k]->num;
          m[i,v[k]] = 1.0;
          m[v[k],i] = 1.0;
        }
      }
    }
    return m;
  } /* END MakeAdjMatrix */

double FindMaxDistance(
   *m: REF AdjMatrix;
   INTEGER n;
  )
      max = 0.0; 
  { for (i = 0; i < n; i++){
      for (j = 0; j <= (i); j++)
      {
        if (m[i,j] > max)
        {
          max = m[i,j];
        }
      }
    }
    return max;
  } /* END FindMaxDistance */
  
AdjMatrix *CalculateStrengh(
   *m : REF AdjMatrix;
   INTEGER n;
   double K;
  )==
  REF ARRAY OF double_vec_t k ;
  { k = NEW(REF ARRAY OF double_vec_t, n, n);
    for (i = 0; i < n; i++)
    {
      for (j = 0; j <= (i); j++)
      {
        if (i == j
        ){  k[i,j] = 0.0;
        }
        else
        {
          k[i,j] = K / (m[i,j]*m[i,j]);
          k[j,i] = k[i,j];
        }  
      }
    }
    return k;
  } /* END CalculateStrengh */

AdjMatrix *CalculateLength(
   *m : REF AdjMatrix;
   INTEGER n;
   double L;
  )==
  REF ARRAY OF double_vec_t l ;
  { l = NEW(REF ARRAY OF double_vec_t, n, n);
    for (i = 0; i < n; i++)
    {
      for (j = 0; j <= (i); j++)
      {
        l[i,j] = L * m[i,j];
        l[j,i] = l[i,j];
      }
    }
    return l;
  } /* END CalculateLength */

/* UNUSED */
void PrintAdjMatrix(*m : REF AdjMatrix; n: INTEGER)

  { for (i = 0; i < n; i++){
      for (j = 0; j < n; j++)
      {
        if (m[i,j] == inf)
        {
          fprintf(stderr, "# ");
        }
        else
        {
          fprintf(stderr, Fmt.LongReal(m[i,j]) & " ");
        }
       }
       fprintf(stderr, "\n");
    }
  } /* END PrintAdjMatrix */

void PrintHalfMatrix(*m : REF AdjMatrix; n: INTEGER)

  { for (i = 0; i < n; i++){
      for (j = 0; j <= (i); j++)
      {
        if (m[i,j] == inf)
        {
          fprintf(stderr, "# ");
        }
        else
        {
          fprintf(stderr, Fmt.LongReal(m[i,j],Fmt.Style.Fix,prec=3) & " ");
        }
      }
      fprintf(stderr, "\n");
    }
  } /* END PrintHalfMatrix */

void ShortestPath(VAR m : REF AdjMatrix; n : INTEGER)
  double *s;     
  { for (i = 0; i < n; i++){
      for (j = 0; j < n; j++)
      {
        if (m[i,j] < inf)
        {
          for (k = 0; k < n; k++)
          {
            if (m[i,k] < inf)
            {
              s = m[j,i] + m[i,k];
              if (s < m[j,k]){ m[j,k] = s; }
            }
          }
        }
      }
    }
  } /* END ShortestPath */
    
void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { ??? NV = erg->top->node.nel;
    ??? F = erg->F;
    ??? eDdif = erg->eDdif^;
    ??? termVar = erg->termVar;
    ??? k = erg->k^;
    ??? l = erg->l^;
    {
      void AccumTerm(*u: uint)
        /*
          Adds to "e" the energy term corresponding to a node "u".
          Returns also the gradient "eDdif".
        */
        {  
          ??? cu = c[u];
          {          
            for (i = u+1; i < NV; i++)
            {
              ??? cv = c[i];
              ??? n = r4_Sub(cu,cv);
              ??? dif = r4_Norm(n);
              ??? luv = l[u,i];
              ??? kuv = k[u,i];
              ??? d   = dif - luv;
              ??? d2  = d * d;
              {
                e = e + F * kuv * d2;
                if (grad)
                {
                  eDdif[u,i] = 2.0 * F * kuv * d;
                  eDdif[i,u] = eDdif[u,i];
                }
                else
                {
                  eDdif[u,i] = 0.0;
                  eDdif[i,u] = eDdif[u,i];
                }
              }
            }          
          }
       } AccumTerm;
      
      void Distribute_eDdif(*u: uint)
        /* Distribute eDdif on endpoints "c[u]" and "c[v]" */
        {
          ??? cu = c[u];
          {  
            for (i = u+1; i < NV; i++)
            {
              ??? ci = c[i];
              ??? n = r4_Sub(cu, ci);
              ??? dif    = r4_Norm(n)+Epsilon;
              ??? eDi    = eDc[i];
              ??? eDu    = eDc[u];
              ??? difDcu = r4_Scale(1.0/dif, n);
              ??? difDci = r4_Scale(-1.0/dif,n);
              ??? eDcu   = r4_Scale(eDdif[u,i], difDcu);
              ??? eDci   = r4_Scale(eDdif[u,i], difDci);
              {
                if (termVar[i])
                {
                  eDi = r4_Add(eDi, eDci);
                }
                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 (l = 0; l < NV; l++)
        {
          if (termVar[l])
          {
              AccumTerm(l);
              if (grad)
              {
                Distribute_eDdif(l);
              }
            }
          }
        }
      }
    } Eval;

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

{
} OriKamSpring.