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

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

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

CONST inf == (uint.nel-1);

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

  SpringList == ARRAY OF Spring;
  Spring     == RECORD u,v : uint; /* extremities of the spring */; }

REVEAL
  T == Public BRANDED OBJECT
      ElemTableRec_t *top;     /* The map elements. */
      termVar: REF bool_vec_t;        /* TRUE if node is variable & existing */
      m : REF AdjacencyMatrix;   /* Matrix of initial distances */
      eDdif: REF double_vec_t_vec_t; /* (Work) Gradient of "e" rel. to "dif"  */
      double L;              /* Kamada' constant */
      spr : REF SpringList;      /* pointer to the list Spring */
                                 
    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;
    {
      erg->m = MakeAdjacencyMatrix(top);
      /*PrintHalfMatrix(erg->m, NV);*/
      ShortestPath(erg->m, NV);
      erg->spr = ChooseSprings(erg->m); 
      /*PrintHalfMatrix(erg->m, NV);*/
      dmax =  FLOAT(FindMaxDistance(erg->m, NV), double);
      erg->L = FLOAT(erg->length,double)/dmax;
      erg->top = top;
      erg->termVar = bool_vec_new(NV);
      erg->eDdif = double_vec_t_vec_new(NV, NV);
      /*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 */


SpringList *ChooseSprings(READOLNY m :  AdjacencyMatrix; dmax: uint)==
 uint ns = 0;
   uint nu,d;
 {
   int NV = (m.nel);
   ??? rs = NEW(REF SpringList, NV*NV), s == rs^;
   ??? ok = NEW(REF ARRAY OF bool_t, NV);
   {
     /* catch distance less equal to 2 */
     for (i = 0; i < NV; i++)
     {
       for (j = 0; j < NV; j++)
       {
         if ((m[i,j] <= MIN(2,dmax)))
         {
           s[ns] = Spring{i,j}; ns++;
         }
       } 
     }
    
     d = 2;
     while (d < dmax)
     {
       /* find a certain nodes that are distance >= "d" apart */
       for (i = 0; i < NV; i++){ ok[i] = TRUE; }
       for (i = 0; i < NV; i++)
       {
         if (ok[i])
         {
           for (j = 0; j < NV; j++)
           {
             if (m[i,j] <= d-1){ ok[j] = FALSE; }  
           }  
         } 
       }
       /* Add spring between those nodes that have a distance less equal to 2*d */
       for (i = 0; i < NV; i++)
       {
         if (ok[i])
         {
           for (j = 0; j < NV; j++)
           {
             if ((ok[j]) && (m[i,j] < 2*d)){ s[ns] = Spring{i,j}; ns++; }  
           }  
         } 
       } 
       d = 2*d;
     }
     ??? rr = NEW(REF SpringList, ns);
     {
       rr^ = SUBARRAY(s,0,ns);
       return rr;
     }
   }   
 } ChooseSprings;

void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /*
      Decide which nodes are relevant to kamada energy. A node is relevant
      iff it is variable.
    */
    int 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 */

AdjacencyMatrix *MakeAdjacencyMatrix(ElemTableRec_t *top, 
 )==
  REF AdjacencyMatrix m ;
  { m = NEW(REF ARRAY OF uint_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;
      }
      m[i,i] = 0;
    }

    for (i = 0; i < top->NE; i++)
    {
      ??? a = top->edge[i].pa;
        Node_t i = OrgV(a)->num;
      Node_t j = OrgV(Clock(a))->num;
      {
        m[i,j] = 1;
      }
    }
    return m;
  } /* END MakeAdjacencyMatrix */

uint FindMaxDistance(
   *m: REF AdjacencyMatrix;
   INTEGER n;
  )
      max = 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 */
  
double CalculateStrength(dist : uint)
  { if (dist == 0){
      return 0.0;
    }
    else
    {
      ??? d = ((double)dist);
      {
        return 1.0/(d*d);
      }
    }
  } /* END CalculateStrength */

double CalculateLength(dist : uint)
  { if (dist == 0){
      return 0.0;
    }
    else
    {
      ??? d = ((double)dist);
      {
        return d;
      }
    }
  } /* END CalculateLength */

/* UNUSED */    
void PrintAdjacencyMatrix(*m : REF AdjacencyMatrix; 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.Int(m[i,j]) & " ");
        }
       }
       fprintf(stderr, "\n");
    }
  } /* END PrintAdjacencyMatrix */

/* UNUSED */
void PrintHalfMatrix(*m : REF AdjacencyMatrix; 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.Int(m[i,j]) & " ");
        }
       }
       fprintf(stderr, "\n");
    }
  } /* END PrintHalfMatrix */

void ShortestPath(VAR m : REF AdjacencyMatrix; n : INTEGER)
  uint *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; }
            }
          }
        }
      }
    }
   } ShortestPath;
    
void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { int NV = erg->top->node.nel;
    ??? eDdif = erg->eDdif^;
    ??? termVar = erg->termVar;
    ??? length = erg->L;
    ??? strength = ((double)erg->strength);
    ??? m = erg->m^;
    {
      void AccumTerm(*u: uint)
        /*
          Adds to "e" the energy term corresponding to a node "u".
          Returns also the gradient "eDdif".
        */
        CONST Epsilon == 1.0d-10; 
        {  
          ??? cu = c[u];
          {    
            for (i = u+1; i < NV; i++)
            {
              ??? cv = c[i];
              ??? n = r4_Sub(cu,cv);
              ??? dif = r4_Norm(n);
              ??? d2  = dif * dif + Epsilon;
              ??? duv = m[u,i];
              ??? luv = length * CalculateLength(duv);
              ??? kuv = strength * CalculateStrength(duv);
              ??? l2  = luv * luv + Epsilon;
              ??? d3  = d2 * dif  + Epsilon;
              {
                e = e + kuv * ( (d2/l2) + (l2/d2) - 2.0);
                if (grad)
                {
                  eDdif[u,i] = 2.0 * kuv * ( (dif/l2) - (l2/d3));
                  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]" */
        CONST Epsilon == 1.0d-10;
        {
          ??? 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
      "PartialSpring(ilenth = " & Fmt.Real(erg->length,Fmt.Style.Fix,prec = 3) \
      " istrength = " & Fmt.Real(erg->strength,Fmt.Style.Fix,prec= 3) & ")";
  } /* END Name */