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


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

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

  
#include <Map.h>
#include <Energy.h>

#include <Octf.h>

CONST inf == 10.0E30;

TYPE
double bool_t = bool_t; bool_vec_t == ARRAY OF bool_t;
double double = double; double_vec_t == double_vec_t;
double NAT = uint; NATS == ARRAY OF NAT;
double DistanceMatrix = ARRAY OF ARRAY OF REAL;
 
REVEAL
double T = Public BRANDED OBJECT
      ElemTableRec_t *top;     /* The map elements. */
      termVar: REF bool_vec_t;        /* TRUE if node is variable & existing */
      m : REF DistanceMatrix;   /* Matrix of initial distances */
      eDdif: REF double_vec_t_vec_t; /* (Work) Gradient of "e" rel. to "dif"  */
      double L;              /* length of 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 = ComputeDistanceMatrix(top);
      /*PrintHalfMatrix(erg->m, NV);*/
      ShortestPath(erg->m, NV);
      /* 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 */
 
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 */

DistanceMatrix *ComputeDistanceMatrix(ElemTableRec_t *top, 
 )==
  REF DistanceMatrix m ;
  { m = NEW(REF ARRAY OF ARRAY OF REAL, 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.0;
    }
    
    ??? cdg = ComputeCorrectedDegrees(top)^;
    {
      for (i = 0; i < top->NE; i++)
      {
        ??? a = top->edge[i].pa;
        Place_t b = Clock(a);
        Node_t i = OrgV(a)->num;
        Node_t j = OrgV(b)->num;
        ??? ni = FLOAT(cdg[i] ,double);
        double nj = FLOAT(cdg[j], double);
        double lij = FLOAT(sqrt(ni/nj + nj/ni), REAL)
        {
          m[i,j] = lij;
          m[j,i] = lij;
        }
      }
    }
    return m;
  } /* END ComputeDistanceMatrix */
  
NATS *ComputeCorrectedDegrees(ElemTableRec_t *top)== 
  /* 
    Computes the corrected degree of each node, defined as 
    the true degree for interior nodes, and the true degree
    plus the interior degree for border nodes. */
  { ??? rd = NEW(REF NATS, top->node.nel), d == rd^;
    double border = bool_vec_new(top->node.nel)^;
    {
      for (i = 0 TO (d.nel-1)){ d[i] = 0; border[i] = FALSE; }
      /* Tally interior @{edge->?}s twice, border edegs once: */
      for (i = 0; i < top->NE; i++)
      {
        ??? a = top->edge[i].pa;
          Place_t b = Clock(a);
        Node_t un = OrgV(a)->num;
        Node_t vn = OrgV(b)->num;
        {
          if ((EdgeIsBorder(a)))
          {
            border[un] = TRUE; border[vn] = TRUE;
            INC(d[un]); INC(d[vn])
          }
          else
          {
            INC(d[un],2); INC(d[vn],2)
          }
        }
      }
      /* Correct count for non-border nodes: */
      for (i = 0 TO (d.nel-1))
      {
        if (! border[i])
        {
          assert(d[i] % 2 == 0);
          d[i] = d[i] DIV 2
        }
      }
      return rd;
    }
  } /* END ComputeCorrectedDegrees */
  
bool_t EdgeIsBorder(Place_t @p)
       Place_t b = a;
  { do {
      if ((PnegP(a) == NULL)){ return TRUE; }
      b = NextF(b);
    } while (b != a);
    return FALSE;
  } /* END EdgeIsBorder */
  
REAL FindMaxDistance(
   *m: REF DistanceMatrix;
   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 */
  
double CalculateStrength(dist: REAL)
  { if (dist == 0.0){
      return 0.0;
    }
    else
    {
      ??? d = FLOAT(dist, double);
      {
        return 1.0/(d*d);
      }
    }
  } /* END CalculateStrength */

double CalculateLength(dist : REAL)
  { if (dist == 0.0){
      return 0.0;
    }
    else
    {
      ??? d = FLOAT(dist, double);
      {
        return d;
      }
    }
  } /* END CalculateLength */

/* UNUSED */
void PrintDistanceMatrix(*m : REF DistanceMatrix; 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.Real(m[i,j], prec=2, style=Fmt.Style.Fix) & " ");
        }
       }
       fprintf(stderr, "\n");
    }
  } /* END PrintDistanceMatrix */

/* UNUSED */
void PrintHalfMatrix(*m : REF DistanceMatrix; 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.Real(m[i,j], prec=2, style=Fmt.Style.Fix) & " ");
        }
       }
       fprintf(stderr, "\n");
    }
  } /* END PrintHalfMatrix */

void ShortestPath(VAR m : REF DistanceMatrix; n : INTEGER)
  REAL *s;     
  { for (i = 0; i < n; i++){
      for (j = 0; j < n; j++)
      {
        if (m[j,i] < 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 = FLOAT(erg->strength,double);
    double m = erg->m^;
    {
      void AccumTerm(*u: NAT)
        /*
          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);
              double dif = r4_Norm(n);
              double d2 = dif * dif + Epsilon;
              double duv = m[u,i];
              double luv = length * CalculateLength(duv);
              double kuv = strength * CalculateStrength(duv);
              double l2  = luv * luv + Epsilon;
              double 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: NAT)
        /* 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
      "VarKamada(length = " & Fmt.Real(erg->length,Fmt.Style.Fix,prec = 3) \
      " strength = " & Fmt.Real(erg->strength,Fmt.Style.Fix,prec= 3) & ")";
  } /* END Name */

  
{
} VarKamSpring.