```/* \$Id: post_process.c,v 1.9 2011/02/28 16:19:43 erg Exp \$ \$Revision: 1.9 \$ */
/* vim:set shiftwidth=4 ts=8: */

/*************************************************************************
* Copyright (c) 2011 AT&T Intellectual Property
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
*
* Contributors: See CVS logs. Details at http://www.graphviz.org/
*************************************************************************/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <time.h>
#include <string.h>
#include <math.h>

#include "types.h"
#include "memory.h"
#include "globals.h"

#include "sparse_solve.h"
#include "post_process.h"
#include "overlap.h"
#include "spring_electrical.h"
#include "call_tri.h"
#include "sfdpinternal.h"

#define node_degree(i) (ia[(i)+1] - ia[(i)])

SparseMatrix ideal_distance_matrix(SparseMatrix A, int dim, real *x){
/* find the ideal distance between edges, either 1, or |N[i] \Union N[j]| - |N[i] \Intersection N[j]|
*/
SparseMatrix D;
int *ia, *ja, i, j, k, l, nz;
real *d;
int *mask = NULL;
real len, di, sum, sumd;

assert(SparseMatrix_is_symmetric(A, FALSE));

D = SparseMatrix_copy(A);
ia = D->ia;
ja = D->ja;
if (D->type != MATRIX_TYPE_REAL){
FREE(D->a);
D->type = MATRIX_TYPE_REAL;
D->a = N_GNEW(D->nz,real);
}
d = (real*) D->a;

for (i = 0; i < D->m; i++) mask[i] = -1;

for (i = 0; i < D->m; i++){
di = node_degree(i);
for (j = ia[i]; j < ia[i+1]; j++){
if (i == ja[j]) continue;
}
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
if (i == k) continue;
len = di + node_degree(k);
for (l = ia[k]; l < ia[k+1]; l++){
if (mask[ja[l]] == i) len--;
}
d[j] = len;
assert(len > 0);
}

}

sum = 0; sumd = 0;
nz = 0;
for (i = 0; i < D->m; i++){
for (j = ia[i]; j < ia[i+1]; j++){
if (i == ja[j]) continue;
nz++;
sum += distance(x, dim, i, ja[j]);
sumd += d[j];
}
}
sum /= nz; sumd /= nz;
sum = sum/sumd;

for (i = 0; i < D->m; i++){
for (j = ia[i]; j < ia[i+1]; j++){
if (i == ja[j]) continue;
d[j] = sum*d[j];
}
}

return D;
}

StressMajorizationSmoother StressMajorizationSmoother2_new(SparseMatrix A, int dim, real lambda0, real *x,
int ideal_dist_scheme){
/* use up to dist 2 neighbor */
StressMajorizationSmoother sm;
int i, j, k, l, m = A->m, *ia = A->ia, *ja = A->ja, *iw, *jw, *id, *jd;
real *d, *w, *lambda;
real *avg_dist, diag_d, diag_w, *dd, dist, s = 0, stop = 0, sbot = 0;
SparseMatrix ID;

assert(SparseMatrix_is_symmetric(A, FALSE));

ID = ideal_distance_matrix(A, dim, x);
dd = (real*) ID->a;

sm = N_GNEW(1,struct StressMajorizationSmoother_struct);
sm->scaling = 1.;
sm->data = NULL;
sm->scheme = SM_SCHEME_NORMAL;

lambda = sm->lambda = N_GNEW(m,real);
for (i = 0; i < m; i++) sm->lambda[i] = lambda0;

avg_dist = N_GNEW(m,real);

for (i = 0; i < m ;i++){
avg_dist[i] = 0;
nz = 0;
for (j = ia[i]; j < ia[i+1]; j++){
if (i == ja[j]) continue;
avg_dist[i] += distance(x, dim, i, ja[j]);
nz++;
}
assert(nz > 0);
avg_dist[i] /= nz;
}

for (i = 0; i < m; i++) mask[i] = -1;

nz = 0;
for (i = 0; i < m; i++){
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
if (mask[k] != i){
nz++;
}
}
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
for (l = ia[k]; l < ia[k+1]; l++){
if (mask[ja[l]] != i){
nz++;
}
}
}
}

sm->Lw = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
sm->Lwd = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
if (!(sm->Lw) || !(sm->Lwd)) {
StressMajorizationSmoother_delete(sm);
return NULL;
}

iw = sm->Lw->ia; jw = sm->Lw->ja;
id = sm->Lwd->ia; jd = sm->Lwd->ja;
w = (real*) sm->Lw->a; d = (real*) sm->Lwd->a;
iw[0] = id[0] = 0;

nz = 0;
for (i = 0; i < m; i++){
diag_d = diag_w = 0;
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
if (mask[k] != i+m){

jw[nz] = k;
if (ideal_dist_scheme == IDEAL_GRAPH_DIST){
dist = 1;
} else if (ideal_dist_scheme == IDEAL_AVG_DIST){
dist = (avg_dist[i] + avg_dist[k])*0.5;
} else if (ideal_dist_scheme == IDEAL_POWER_DIST){
dist = pow(distance_cropped(x,dim,i,k),.4);
} else {
fprintf(stderr,"ideal_dist_scheme value wrong");
assert(0);
exit(1);
}

/*
w[nz] = -1./(ia[i+1]-ia[i]+ia[ja[j]+1]-ia[ja[j]]);
w[nz] = -2./(avg_dist[i]+avg_dist[k]);*/
/* w[nz] = -1.;*//* use unit weight for now, later can try 1/(deg(i)+deg(k)) */
w[nz] = -1/(dist*dist);

diag_w += w[nz];

jd[nz] = k;
/*
d[nz] = w[nz]*distance(x,dim,i,k);
d[nz] = w[nz]*dd[j];
d[nz] = w[nz]*(avg_dist[i] + avg_dist[k])*0.5;
*/
d[nz] = w[nz]*dist;
stop += d[nz]*distance(x,dim,i,k);
sbot += d[nz]*dist;
diag_d += d[nz];

nz++;
}
}

/* distance 2 neighbors */
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
for (l = ia[k]; l < ia[k+1]; l++){
if (mask[ja[l]] != i+m){

if (ideal_dist_scheme == IDEAL_GRAPH_DIST){
dist = 2;
} else if (ideal_dist_scheme == IDEAL_AVG_DIST){
dist = (avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]])*0.5;
} else if (ideal_dist_scheme == IDEAL_POWER_DIST){
dist = pow(distance_cropped(x,dim,i,ja[l]),.4);
} else {
fprintf(stderr,"ideal_dist_scheme value wrong");
assert(0);
exit(1);
}

jw[nz] = ja[l];
/*
w[nz] = -1/(ia[i+1]-ia[i]+ia[ja[l]+1]-ia[ja[l]]);
w[nz] = -2/(avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]]);*/
/* w[nz] = -1.;*//* use unit weight for now, later can try 1/(deg(i)+deg(k)) */

w[nz] = -1/(dist*dist);

diag_w += w[nz];

jd[nz] = ja[l];
/*
d[nz] = w[nz]*(distance(x,dim,i,k)+distance(x,dim,k,ja[l]));
d[nz] = w[nz]*(dd[j]+dd[l]);
d[nz] = w[nz]*(avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]])*0.5;
*/
d[nz] = w[nz]*dist;
stop += d[nz]*distance(x,dim,ja[l],k);
sbot += d[nz]*dist;
diag_d += d[nz];

nz++;
}
}
}
jw[nz] = i;
lambda[i] *= (-diag_w);/* alternatively don't do that then we have a constant penalty term scaled by lambda0 */

w[nz] = -diag_w + lambda[i];
jd[nz] = i;
d[nz] = -diag_d;
nz++;

iw[i+1] = nz;
id[i+1] = nz;
}
s = stop/sbot;
for (i = 0; i < nz; i++) d[i] *= s;

sm->scaling = s;
sm->Lw->nz = nz;
sm->Lwd->nz = nz;

FREE(avg_dist);
SparseMatrix_delete(ID);
return sm;
}

StressMajorizationSmoother SparseStressMajorizationSmoother_new(SparseMatrix A, int dim, real lambda0, real *x, int weighting_scheme){
/* solve a stress model to achieve the ideal distance among a sparse set of edges recorded in A.
A must be a real matrix.
*/
StressMajorizationSmoother sm;
int i, j, k, m = A->m, *ia, *ja, *iw, *jw, *id, *jd;
int nz;
real *d, *w, *lambda;
real diag_d, diag_w, *a, dist, s = 0, stop = 0, sbot = 0;
real xdot = 0;

assert(SparseMatrix_is_symmetric(A, FALSE) && A->type == MATRIX_TYPE_REAL);

/* if x is all zero, make it random */
for (i = 0; i < m*dim; i++) xdot += x[i]*x[i];
if (xdot == 0){
for (i = 0; i < m*dim; i++) x[i] = 72*drand();
}

ia = A->ia;
ja = A->ja;
a = (real*) A->a;

sm = MALLOC(sizeof(struct StressMajorizationSmoother_struct));
sm->scaling = 1.;
sm->data = NULL;
sm->scheme = SM_SCHEME_NORMAL;

lambda = sm->lambda = MALLOC(sizeof(real)*m);
for (i = 0; i < m; i++) sm->lambda[i] = lambda0;

nz = A->nz;

sm->Lw = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
sm->Lwd = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
if (!(sm->Lw) || !(sm->Lwd)) {
StressMajorizationSmoother_delete(sm);
return NULL;
}

iw = sm->Lw->ia; jw = sm->Lw->ja;
id = sm->Lwd->ia; jd = sm->Lwd->ja;
w = (real*) sm->Lw->a; d = (real*) sm->Lwd->a;
iw[0] = id[0] = 0;

nz = 0;
for (i = 0; i < m; i++){
diag_d = diag_w = 0;
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
if (k != i){

jw[nz] = k;
dist = a[j];
switch (weighting_scheme){
case WEIGHTING_SCHEME_SQR_DIST:
if (dist*dist == 0){
w[nz] = -100000;
} else {
w[nz] = -1/(dist*dist);
}
break;
case WEIGHTING_SCHEME_NONE:
w[nz] = -1;
break;
default:
assert(0);
return NULL;
}
diag_w += w[nz];
jd[nz] = k;
d[nz] = w[nz]*dist;

stop += d[nz]*distance(x,dim,i,k);
sbot += d[nz]*dist;
diag_d += d[nz];

nz++;
}
}

jw[nz] = i;
lambda[i] *= (-diag_w);/* alternatively don't do that then we have a constant penalty term scaled by lambda0 */
w[nz] = -diag_w + lambda[i];

jd[nz] = i;
d[nz] = -diag_d;
nz++;

iw[i+1] = nz;
id[i+1] = nz;
}
s = stop/sbot;
if (s == 0) {
return NULL;
}
for (i = 0; i < nz; i++) d[i] *= s;

sm->scaling = s;
sm->Lw->nz = nz;
sm->Lwd->nz = nz;

return sm;
}

static real total_distance(int m, int dim, real* x, real* y){
real total = 0, dist = 0;
int i, j;

for (i = 0; i < m; i++){
dist = 0.;
for (j = 0; j < dim; j++){
dist += (y[i*dim+j] - x[i*dim+j])*(y[i*dim+j] - x[i*dim+j]);
}
total += sqrt(dist);
}

}

void SparseStressMajorizationSmoother_delete(SparseStressMajorizationSmoother sm){
return StressMajorizationSmoother_delete(sm);
}

real SparseStressMajorizationSmoother_smooth(SparseStressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol){

return StressMajorizationSmoother_smooth(sm, dim, x, maxit_sm, tol);

}
static void get_edge_label_matrix(relative_position_constraints data, int m, int dim, real *x, SparseMatrix *LL, real **rhs){
int edge_labeling_scheme = data->edge_labeling_scheme;
int n_constr_nodes = data->n_constr_nodes;
int *constr_nodes = data->constr_nodes;
SparseMatrix A_constr = data->A_constr;
int *ia = A_constr->ia, *ja = A_constr->ja, ii, jj, nz, l, ll, i, j;
int *irn = data->irn, *jcn = data->jcn;
real *val = data->val, dist, kk, k;
real *x00 = NULL;
SparseMatrix Lc = NULL;
real constr_penalty = data->constr_penalty;

if (edge_labeling_scheme == ELSCHEME_PENALTY || edge_labeling_scheme == ELSCHEME_STRAIGHTLINE_PENALTY){
/* for an node with two neighbors j--i--k, and assume i needs to be between j and k, then the contribution to P is
.   i    j     k
i   1   -0.5  -0.5
j  -0.5 0.25  0.25
k  -0.5 0.25  0.25
in general, if i has m neighbors j, k, ..., then
p_ii = 1
p_jj = 1/m^2
p_ij = -1/m
p jk = 1/m^2
.    i     j    k
i    1    -1/m  -1/m ...
j   -1/m  1/m^2 1/m^2 ...
k   -1/m  1/m^2 1/m^2 ...
*/
if (!irn){
assert((!jcn) && (!val));
nz = 0;
for (i = 0; i < n_constr_nodes; i++){
ii = constr_nodes[i];
k = ia[ii+1] - ia[ii];/*usually k = 2 */
nz += (k+1)*(k+1);

}
irn = data->irn = MALLOC(sizeof(int)*nz);
jcn = data->jcn = MALLOC(sizeof(int)*nz);
val = data->val = MALLOC(sizeof(double)*nz);
}
nz = 0;
for (i = 0; i < n_constr_nodes; i++){
ii = constr_nodes[i];
jj = ja[ia[ii]]; ll = ja[ia[ii] + 1];
if (jj == ll) continue; /* do not do loops */
dist = distance_cropped(x, dim, jj, ll);
dist *= dist;

k = ia[ii+1] - ia[ii];/* usually k = 2 */
kk = k*k;
irn[nz] = ii; jcn[nz] = ii; val[nz++] = constr_penalty/(dist);
k = constr_penalty/(k*dist); kk = constr_penalty/(kk*dist);
for (j = ia[ii]; j < ia[ii+1]; j++){
irn[nz] = ii; jcn[nz] = ja[j]; val[nz++] = -k;
}
for (j = ia[ii]; j < ia[ii+1]; j++){
jj = ja[j];
irn[nz] = jj; jcn[nz] = ii; val[nz++] = -k;
for (l = ia[ii]; l < ia[ii+1]; l++){
ll = ja[l];
irn[nz] = jj; jcn[nz] = ll; val[nz++] = kk;
}
}
}
Lc = SparseMatrix_from_coordinate_arrays(nz, m, m, irn, jcn, val, MATRIX_TYPE_REAL);
} else if (edge_labeling_scheme == ELSCHEME_PENALTY2 || edge_labeling_scheme == ELSCHEME_STRAIGHTLINE_PENALTY2){
/* for an node with two neighbors j--i--k, and assume i needs to be between the old position of j and k, then the contribution to P is
1/d_jk, and to the right hand side: {0,...,average_position_of_i's neighbor if i is an edge node,...}
*/
if (!irn){
assert((!jcn) && (!val));
nz = n_constr_nodes;
irn = data->irn = MALLOC(sizeof(int)*nz);
jcn = data->jcn = MALLOC(sizeof(int)*nz);
val = data->val = MALLOC(sizeof(double)*nz);
}
x00 = MALLOC(sizeof(real)*m*dim);
for (i = 0; i < m*dim; i++) x00[i] = 0;
nz = 0;
for (i = 0; i < n_constr_nodes; i++){
ii = constr_nodes[i];
jj = ja[ia[ii]]; ll = ja[ia[ii] + 1];
dist = distance_cropped(x, dim, jj, ll);
irn[nz] = ii; jcn[nz] = ii; val[nz++] = constr_penalty/(dist);
for (j = ia[ii]; j < ia[ii+1]; j++){
jj = ja[j];
for (l = 0; l < dim; l++){
x00[ii*dim+l] += x[jj*dim+l];
}
}
for (l = 0; l < dim; l++) {
x00[ii*dim+l] *= constr_penalty/(dist)/(ia[ii+1] - ia[ii]);
}
}
Lc = SparseMatrix_from_coordinate_arrays(nz, m, m, irn, jcn, val, MATRIX_TYPE_REAL);

}
*LL = Lc;
*rhs = x00;
}

real get_stress(int m, int dim, int *iw, int *jw, real *w, real *d, real *x, real scaling, void *data, int weighted){
int i, j;
real res = 0., dist;
/* we use the fact that d_ij = w_ij*dist(i,j). Also, d_ij and x are scalinged by *scaling, so divide by it to get actual unscaled streee. */
for (i = 0; i < m; i++){
for (j = iw[i]; j < iw[i+1]; j++){
if (i == jw[j]) {
continue;
}
dist = d[j]/w[j];/* both negative*/
if (weighted){
res += -w[j]*(dist - distance(x, dim, i, jw[j]))*(dist - distance(x, dim, i, jw[j]));
} else {
res += (dist - distance(x, dim, i, jw[j]))*(dist - distance(x, dim, i, jw[j]));
}
}
}
return res/scaling/scaling;

}

static void uniform_stress_augment_rhs(int m, int dim, real *x, real *y, real alpha, real M){
int i, j, k;
real dist, distij;
for (i = 0; i < m; i++){
for (j = i+1; j < m; j++){
dist = distance_cropped(x, dim, i, j);
for (k = 0; k < dim; k++){
distij = (x[i*dim+k] - x[j*dim+k])/dist;
y[i*dim+k] += alpha*M*distij;
y[j*dim+k] += alpha*M*(-distij);
}
}
}
}

static real uniform_stress_solve(SparseMatrix Lw, real alpha, int dim, real *x0, real *rhs, real tol, int maxit, int *flag){
Operator Ax;
Operator Precon;

Ax = Operator_uniform_stress_matmul(Lw, alpha);
Precon = Operator_uniform_stress_diag_precon_new(Lw, alpha);

return cg(Ax, Precon, Lw->m, dim, x0, rhs, tol, maxit, flag);

}

real StressMajorizationSmoother_smooth(StressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol) {
SparseMatrix Lw = sm->Lw, Lwd = sm->Lwd, Lwdd = NULL;
int i, j, m, *id, *jd, *iw, *jw, idiag, flag = 0, iter = 0;
real *w, *dd, *d, *y = NULL, *x0 = NULL, *x00 = NULL, diag, diff = 1, *lambda = sm->lambda, maxit, res, alpha = 0., M = 0.;
SparseMatrix Lc = NULL;

Lwdd = SparseMatrix_copy(Lwd);
m = Lw->m;
x0 = N_GNEW(dim*m,real);
if (!x0) goto RETURN;

x0 = MEMCPY(x0, x, sizeof(real)*dim*m);
y = N_GNEW(dim*m,real);
if (!y) goto RETURN;

id = Lwd->ia; jd = Lwd->ja;
d = (real*) Lwd->a;
dd = (real*) Lwdd->a;
w = (real*) Lw->a;
iw = Lw->ia; jw = Lw->ja;

/* for the additional matrix L due to the position constraints */
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
get_edge_label_matrix(sm->data, m, dim, x, &Lc, &x00);
if (Lc) Lw = SparseMatrix_add(Lw, Lc);
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
alpha = ((real*) (sm->data))[0];
M = ((real*) (sm->data))[1];
}

while (iter++ < maxit_sm && diff > tol){

for (i = 0; i < m; i++){
idiag = -1;
diag = 0.;
for (j = id[i]; j < id[i+1]; j++){
if (i == jd[j]) {
idiag = j;
continue;
}
dd[j] = d[j]/distance_cropped(x, dim, i, jd[j]);
diag += dd[j];
}
assert(idiag >= 0);
dd[idiag] = -diag;
}

/* solve (Lw+lambda*I) x = Lwdd y + lambda x0 */

SparseMatrix_multiply_dense(Lwdd, FALSE, x, FALSE, &y, FALSE, dim);

if (lambda){/* is there a penalty term? */
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += lambda[i]*x0[i*dim+j];
}
}
}

if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += x00[i*dim+j];
}
}
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){/* this part can be done more efficiently using octree approximation */
uniform_stress_augment_rhs(m, dim, x, y, alpha, M);
}

#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress1 = %f\n",get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 0));
#endif

maxit = sqrt((double) m);
if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
res = uniform_stress_solve(Lw, alpha, dim, x, y, 0.01, maxit, &flag);
} else {
res = SparseMatrix_solve(Lw, dim, x, y, 0.01, maxit, SOLVE_METHOD_CG, &flag);
}
if (flag) goto RETURN;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress2 = %f\n",get_stress(m, dim, iw, jw, w, d, y, sm->scaling, sm->data, 0));
#endif

diff = total_distance(m, dim, x, y)/sqrt(vector_product(m*dim, x, x));
#ifdef DEBUG_PRINT
if (Verbose){
fprintf(stderr, "Outer iter = %d, res = %g Stress Majorization diff = %g\n",iter, res, diff);
}
#endif
MEMCPY(x, y, sizeof(real)*m*dim);
}

#ifdef DEBUG
_statistics[1] += iter-1;
#endif

RETURN:
SparseMatrix_delete(Lwdd);
if (Lc) {
SparseMatrix_delete(Lc);
SparseMatrix_delete(Lw);
}

if (x0) FREE(x0);
if (y) FREE(y);
if (x00) FREE(x00);
return diff;

}

void StressMajorizationSmoother_delete(StressMajorizationSmoother sm){
if (!sm) return;
if (sm->Lw) SparseMatrix_delete(sm->Lw);
if (sm->Lwd) SparseMatrix_delete(sm->Lwd);
if (sm->lambda) FREE(sm->lambda);
if (sm->data) sm->data_deallocator(sm->data);
FREE(sm);
}

TriangleSmoother TriangleSmoother_new(SparseMatrix A, int dim, real lambda0, real *x, int use_triangularization){
TriangleSmoother sm;
int i, j, k, m = A->m, *ia = A->ia, *ja = A->ja, *iw, *jw, *id, *jd, jdiag, nz;
SparseMatrix B;
real *avg_dist, *lambda, *d, *w, diag_d, diag_w, dist;
real s = 0, stop = 0, sbot = 0;

assert(SparseMatrix_is_symmetric(A, FALSE));

avg_dist = N_GNEW(m,real);

for (i = 0; i < m ;i++){
avg_dist[i] = 0;
nz = 0;
for (j = ia[i]; j < ia[i+1]; j++){
if (i == ja[j]) continue;
avg_dist[i] += distance(x, dim, i, ja[j]);
nz++;
}
assert(nz > 0);
avg_dist[i] /= nz;
}

sm = N_GNEW(1,struct TriangleSmoother_struct);
sm->scaling = 1;
sm->data = NULL;
sm->scheme = SM_SCHEME_NORMAL;

lambda = sm->lambda = N_GNEW(m,real);
for (i = 0; i < m; i++) sm->lambda[i] = lambda0;

if (m > 2){
if (use_triangularization){
B= call_tri(m, dim, x);
} else {
B= call_tri2(m, dim, x);
}
} else {
B = SparseMatrix_copy(A);
}

sm->Lw = SparseMatrix_add(A, B);

SparseMatrix_delete(B);
sm->Lwd = SparseMatrix_copy(sm->Lw);
if (!(sm->Lw) || !(sm->Lwd)) {
TriangleSmoother_delete(sm);
return NULL;
}

iw = sm->Lw->ia; jw = sm->Lw->ja;
id = sm->Lwd->ia; jd = sm->Lwd->ja;
w = (real*) sm->Lw->a; d = (real*) sm->Lwd->a;

for (i = 0; i < m; i++){
diag_d = diag_w = 0;
jdiag = -1;
for (j = iw[i]; j < iw[i+1]; j++){
k = jw[j];
if (k == i){
jdiag = j;
continue;
}
/*      w[j] = -1./(ia[i+1]-ia[i]+ia[ja[j]+1]-ia[ja[j]]);
w[j] = -2./(avg_dist[i]+avg_dist[k]);
w[j] = -1.*/;/* use unit weight for now, later can try 1/(deg(i)+deg(k)) */
dist = pow(distance_cropped(x,dim,i,k),0.6);
w[j] = 1/(dist*dist);
diag_w += w[j];

/*      d[j] = w[j]*distance(x,dim,i,k);
d[j] = w[j]*(avg_dist[i] + avg_dist[k])*0.5;*/
d[j] = w[j]*dist;
stop += d[j]*distance(x,dim,i,k);
sbot += d[j]*dist;
diag_d += d[j];

}

lambda[i] *= (-diag_w);/* alternatively don't do that then we have a constant penalty term scaled by lambda0 */

assert(jdiag >= 0);
w[jdiag] = -diag_w + lambda[i];
d[jdiag] = -diag_d;
}

s = stop/sbot;
for (i = 0; i < iw[m]; i++) d[i] *= s;
sm->scaling = s;

FREE(avg_dist);

return sm;
}

void TriangleSmoother_delete(TriangleSmoother sm){

StressMajorizationSmoother_delete(sm);

}

void TriangleSmoother_smooth(TriangleSmoother sm, int dim, real *x){

StressMajorizationSmoother_smooth(sm, dim, x, 50, 0.001);
}

/* ================================ spring and spring-electrical based smoother ================ */
SpringSmoother SpringSmoother_new(SparseMatrix A, int dim, spring_electrical_control ctrl, real *x){
SpringSmoother sm;
int i, j, k, l, m = A->m, *ia = A->ia, *ja = A->ja, *id, *jd;
real *d, *dd;
real *avg_dist;
SparseMatrix ID = NULL;

assert(SparseMatrix_is_symmetric(A, FALSE));

ID = ideal_distance_matrix(A, dim, x);
dd = (real*) ID->a;

sm = N_GNEW(1,struct SpringSmoother_struct);

avg_dist = N_GNEW(m,real);

for (i = 0; i < m ;i++){
avg_dist[i] = 0;
nz = 0;
for (j = ia[i]; j < ia[i+1]; j++){
if (i == ja[j]) continue;
avg_dist[i] += distance(x, dim, i, ja[j]);
nz++;
}
assert(nz > 0);
avg_dist[i] /= nz;
}

for (i = 0; i < m; i++) mask[i] = -1;

nz = 0;
for (i = 0; i < m; i++){
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
if (mask[k] != i){
nz++;
}
}
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
for (l = ia[k]; l < ia[k+1]; l++){
if (mask[ja[l]] != i){
nz++;
}
}
}
}

sm->D = SparseMatrix_new(m, m, nz, MATRIX_TYPE_REAL, FORMAT_CSR);
if (!(sm->D)){
SpringSmoother_delete(sm);
return NULL;
}

id = sm->D->ia; jd = sm->D->ja;
d = (real*) sm->D->a;
id[0] = 0;

nz = 0;
for (i = 0; i < m; i++){
for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
if (mask[k] != i+m){
jd[nz] = k;
d[nz] = (avg_dist[i] + avg_dist[k])*0.5;
d[nz] = dd[j];
nz++;
}
}

for (j = ia[i]; j < ia[i+1]; j++){
k = ja[j];
for (l = ia[k]; l < ia[k+1]; l++){
if (mask[ja[l]] != i+m){
jd[nz] = ja[l];
d[nz] = (avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]])*0.5;
d[nz] = dd[j]+dd[l];
nz++;
}
}
}
id[i+1] = nz;
}
sm->D->nz = nz;
sm->ctrl = spring_electrical_control_new();
*(sm->ctrl) = *ctrl;
sm->ctrl->random_start = FALSE;
sm->ctrl->multilevels = 1;
sm->ctrl->step /= 2;
sm->ctrl->maxiter = 20;

FREE(avg_dist);
SparseMatrix_delete(ID);

return sm;
}

void SpringSmoother_delete(SpringSmoother sm){
if (!sm) return;
if (sm->D) SparseMatrix_delete(sm->D);
if (sm->ctrl) spring_electrical_control_delete(sm->ctrl);
}

void SpringSmoother_smooth(SpringSmoother sm, SparseMatrix A, real *node_weights, int dim, real *x){
int flag = 0;

spring_electrical_spring_embedding(dim, A, sm->D, sm->ctrl, node_weights, x, &flag);
assert(!flag);

}

/*=============================== end of spring and spring-electrical based smoother =========== */

void post_process_smoothing(int dim, SparseMatrix A, spring_electrical_control ctrl, real *node_weights, real *x, int *flag){
#ifdef TIME
clock_t  cpu;
#endif

#ifdef TIME
cpu = clock();
#endif
*flag = 0;

switch (ctrl->smoothing){
case SMOOTHING_RNG:
case SMOOTHING_TRIANGLE:{
TriangleSmoother sm;

if (ctrl->smoothing == SMOOTHING_RNG){
sm = TriangleSmoother_new(A, dim, 0, x, FALSE);
} else {
sm = TriangleSmoother_new(A, dim, 0, x, TRUE);
}
TriangleSmoother_smooth(sm, dim, x);
TriangleSmoother_delete(sm);
break;
}
case SMOOTHING_STRESS_MAJORIZATION_GRAPH_DIST:
case SMOOTHING_STRESS_MAJORIZATION_POWER_DIST:
case SMOOTHING_STRESS_MAJORIZATION_AVG_DIST:
{
StressMajorizationSmoother sm;
int k, dist_scheme = IDEAL_AVG_DIST;

if (ctrl->smoothing == SMOOTHING_STRESS_MAJORIZATION_GRAPH_DIST){
dist_scheme = IDEAL_GRAPH_DIST;
} else if (ctrl->smoothing == SMOOTHING_STRESS_MAJORIZATION_AVG_DIST){
dist_scheme = IDEAL_AVG_DIST;
} else if (ctrl->smoothing == SMOOTHING_STRESS_MAJORIZATION_POWER_DIST){
dist_scheme = IDEAL_POWER_DIST;
}

for (k = 0; k < 1; k++){
sm = StressMajorizationSmoother2_new(A, dim, 0.05, x, dist_scheme);
StressMajorizationSmoother_smooth(sm, dim, x, 50, 0.001);
StressMajorizationSmoother_delete(sm);
}
break;
}
case SMOOTHING_SPRING:{
SpringSmoother sm;
int k;

for (k = 0; k < 1; k++){
sm = SpringSmoother_new(A, dim, ctrl, x);
SpringSmoother_smooth(sm, A, node_weights, dim, x);
SpringSmoother_delete(sm);
}

break;
}

}/* end switch between smoothing methods */

#ifdef TIME
if (Verbose) fprintf(stderr, "post processing %f\n",((real) (clock() - cpu)) / CLOCKS_PER_SEC);
#endif
}
```