/* -*- mode: C -*-  */
/* vim:set ts=2 sw=2 sts=2 et: */
/* 
   IGraph R package.
   Copyright (C) 2014  Gabor Csardi <csardi.gabor@gmail.com>
   334 Harvard street, Cambridge, MA 02139 USA
   
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
   
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   
   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 
   02110-1301 USA

*/

#include "igraph_layout.h"
#include "igraph_interface.h"
#include "igraph_paths.h"
#include "igraph_random.h"

/**
 * \ingroup layout
 * \function igraph_layout_kamada_kawai
 * \brief Places the vertices on a plane according the Kamada-Kawai algorithm. 
 *
 * </para><para>
 * This is a force directed layout, see  Kamada, T. and Kawai, S.: An
 * Algorithm for Drawing General Undirected Graphs. Information
 * Processing Letters, 31/1, 7--15, 1989.
 * \param graph A graph object.
 * \param res Pointer to an initialized matrix object. This will
 *        contain the result (x-positions in column zero and
 *        y-positions in column one) and will be resized if needed.
 * \param use_seed Boolean, whether to use the values supplied in the
 *        \p res argument as the initial configuration. If zero then a
 *        random initial configuration is used.
 * \param maxiter The maximum number of iterations to perform. A reasonable
 *        default value is at least ten (or more) times the number of 
 *        vertices.  
 * \param epsilon Stop the iteration, if the maximum delta value of the 
 *        algorithm is smaller than still. It is safe to leave it at zero, 
 *        and then \p maxiter iterations are performed.
 * \param kkconst The Kamada-Kawai vertex attraction constant.
 *        Typical value: number of vertices.
 * \param weights Edge weights, larger values will result longer edges.
 * \param minx Pointer to a vector, or a \c NULL pointer. If not a 
 *        \c NULL pointer then the vector gives the minimum
 *        \quote x \endquote coordinate for every vertex.
 * \param maxx Same as \p minx, but the maximum \quote x \endquote 
 *        coordinates.
 * \param miny Pointer to a vector, or a \c NULL pointer. If not a 
 *        \c NULL pointer then the vector gives the minimum
 *        \quote y \endquote coordinate for every vertex.
 * \param maxy Same as \p miny, but the maximum \quote y \endquote 
 *        coordinates.
 * \return Error code.
 * 
 * Time complexity: O(|V|) for each iteration, after an O(|V|^2
 * log|V|) initialization step. |V| is the number of vertices in the
 * graph.
 */

int igraph_layout_kamada_kawai(const igraph_t *graph, igraph_matrix_t *res,
	       igraph_bool_t use_seed, igraph_integer_t maxiter,
	       igraph_real_t epsilon, igraph_real_t kkconst, 
	       const igraph_vector_t *weights,
	       const igraph_vector_t *minx, const igraph_vector_t *maxx,
	       const igraph_vector_t *miny, const igraph_vector_t *maxy) {
  
  igraph_integer_t no_nodes=igraph_vcount(graph);
  igraph_integer_t no_edges=igraph_ecount(graph);
  igraph_real_t L, L0=sqrt(no_nodes);  
  igraph_matrix_t dij, lij, kij;
  igraph_real_t max_dij;
  igraph_vector_t D1, D2;
  igraph_integer_t i, j, m;

  if (maxiter < 0) {
    IGRAPH_ERROR("Number of iterations must be non-negatice in "
		 "Kamada-Kawai layout", IGRAPH_EINVAL);
  }
  if (kkconst <= 0) {
    IGRAPH_ERROR("`K' constant must be positive in Kamada-Kawai layout",
		 IGRAPH_EINVAL);
  }

  if (use_seed && (igraph_matrix_nrow(res) != no_nodes ||
		   igraph_matrix_ncol(res) != 2)) {
    IGRAPH_ERROR("Invalid start position matrix size in "
		 "Kamada-Kawai layout", IGRAPH_EINVAL);
  }
  if (weights && igraph_vector_size(weights) != no_edges) {
    IGRAPH_ERROR("Invalid weight vector length", IGRAPH_EINVAL);
  }

  if (minx && igraph_vector_size(minx) != no_nodes) {
    IGRAPH_ERROR("Invalid minx vector length", IGRAPH_EINVAL);
  }
  if (maxx && igraph_vector_size(maxx) != no_nodes) {
    IGRAPH_ERROR("Invalid maxx vector length", IGRAPH_EINVAL);
  }
  if (minx && maxx && !igraph_vector_all_le(minx, maxx)) {
    IGRAPH_ERROR("minx must not be greater than maxx", IGRAPH_EINVAL);
  }
  if (miny && igraph_vector_size(miny) != no_nodes) {
    IGRAPH_ERROR("Invalid miny vector length", IGRAPH_EINVAL);
  }
  if (maxy && igraph_vector_size(maxy) != no_nodes) {
    IGRAPH_ERROR("Invalid maxy vector length", IGRAPH_EINVAL);
  }
  if (miny && maxy && !igraph_vector_all_le(miny, maxy)) {
    IGRAPH_ERROR("miny must not be greater than maxy", IGRAPH_EINVAL);
  }

  if (!use_seed) {
    if (minx || maxx || miny || maxy) {
      const igraph_real_t width=sqrt(no_nodes), height=width;
      IGRAPH_CHECK(igraph_matrix_resize(res, no_nodes, 2));
      RNG_BEGIN();
      for (i=0; i<no_nodes; i++) {
	igraph_real_t x1=minx ? VECTOR(*minx)[i] : -width/2;
	igraph_real_t x2=maxx ? VECTOR(*maxx)[i] :  width/2;
	igraph_real_t y1=miny ? VECTOR(*miny)[i] : -height/2;
	igraph_real_t y2=maxy ? VECTOR(*maxy)[i] :  height/2;
	if (!igraph_finite(x1)) { x1 = -width/2; }
	if (!igraph_finite(x2)) { x2 =  width/2; }
	if (!igraph_finite(y1)) { y1 = -height/2; }
	if (!igraph_finite(y2)) { y2 =  height/2; }
	MATRIX(*res, i, 0) = RNG_UNIF(x1, x2);
	MATRIX(*res, i, 1) = RNG_UNIF(y1, y2);
      }
      RNG_END();
    } else {
      igraph_layout_circle(graph, res, /* order= */ igraph_vss_all());
    }
  }

  if (no_nodes <= 1) { return 0; }

  IGRAPH_MATRIX_INIT_FINALLY(&dij, no_nodes, no_nodes);
  IGRAPH_MATRIX_INIT_FINALLY(&kij, no_nodes, no_nodes);
  IGRAPH_MATRIX_INIT_FINALLY(&lij, no_nodes, no_nodes);

  if (weights && igraph_vector_min(weights) < 0) {
    IGRAPH_CHECK(igraph_shortest_paths_bellman_ford(graph, &dij, igraph_vss_all(),
						    igraph_vss_all(), weights,
						    IGRAPH_ALL));
  } else {

    IGRAPH_CHECK(igraph_shortest_paths_dijkstra(graph, &dij, igraph_vss_all(),
						igraph_vss_all(), weights,
						IGRAPH_ALL));
  }
  
  max_dij = 0.0;
  for (i=0; i<no_nodes; i++) {
    for (j=i+1; j<no_nodes; j++) {
      if (!igraph_finite(MATRIX(dij, i, j))) { continue; }
      if (MATRIX(dij, i, j) > max_dij) { max_dij = MATRIX(dij, i, j); }
    }
  }
  for (i=0; i<no_nodes; i++) {
    for (j=0; j<no_nodes; j++) {
      if (MATRIX(dij, i, j) > max_dij) { MATRIX(dij, i, j) = max_dij; }
    }
  }

  L = L0 / max_dij;
  for (i=0; i<no_nodes; i++) {
    for (j=0; j<no_nodes; j++) {
      igraph_real_t tmp=MATRIX(dij, i, j) * MATRIX(dij, i, j);
      if (i==j) { continue; }
      MATRIX(kij, i, j) = kkconst / tmp;
      MATRIX(lij, i, j) = L * MATRIX(dij, i, j);
    }
  }

  /* Initialize delta */
  IGRAPH_VECTOR_INIT_FINALLY(&D1, no_nodes);
  IGRAPH_VECTOR_INIT_FINALLY(&D2, no_nodes);
  for (m=0; m<no_nodes; m++) {
    igraph_real_t myD1=0.0, myD2=0.0;
    for (i=0; i<no_nodes; i++) { 
      igraph_real_t dx, dy, mi_dist;
      if (i==m) { continue; }
      dx=MATRIX(*res, m, 0) - MATRIX(*res, i, 0);
      dy=MATRIX(*res, m, 1) - MATRIX(*res, i, 1);
      mi_dist=sqrt(dx * dx + dy * dy);
      myD1 += MATRIX(kij, m, i) * (dx - MATRIX(lij, m, i) * dx / mi_dist);
      myD2 += MATRIX(kij, m, i) * (dy - MATRIX(lij, m, i) * dy / mi_dist);
    }
    VECTOR(D1)[m] = myD1;
    VECTOR(D2)[m] = myD2;
  }

  for (j=0; j<maxiter; j++) {
    igraph_real_t myD1, myD2, A, B, C;
    igraph_real_t max_delta, delta_x, delta_y;
    igraph_real_t old_x, old_y, new_x, new_y;
    
    myD1=0.0, myD2=0.0, A=0.0, B=0.0, C=0.0;
    
    /* Select maximal delta */
    m=0; max_delta=-1;
    for (i=0; i<no_nodes; i++) {
      igraph_real_t delta=(VECTOR(D1)[i] * VECTOR(D1)[i] + 
			   VECTOR(D2)[i] * VECTOR(D2)[i]);
      if (delta > max_delta) { 
	m=i; max_delta=delta;
      }
    }
    if (max_delta < epsilon) { break; }
    old_x=MATRIX(*res, m, 0);
    old_y=MATRIX(*res, m, 1);
    
    /* Calculate D1 and D2, A, B, C */
    for (i=0; i<no_nodes; i++) {
      igraph_real_t dx, dy, dist, den;
      if (i==m) { continue; }
      dx=old_x - MATRIX(*res, i, 0);
      dy=old_y - MATRIX(*res, i, 1);
      dist=sqrt(dx * dx + dy * dy);
      den=dist * (dx * dx + dy * dy);
      A += MATRIX(kij, m, i) * (1 - MATRIX(lij, m, i) * dy * dy / den);
      B += MATRIX(kij, m, i) * MATRIX(lij, m, i) * dx * dy / den;
      C += MATRIX(kij, m, i) * (1 - MATRIX(lij, m, i) * dx * dx / den);
    }
    myD1 = VECTOR(D1)[m];
    myD2 = VECTOR(D2)[m];

    /* Need to solve some linear equations */
    delta_y = (B * myD1 - myD2 * A) / (C * A - B * B);
    delta_x = - (myD1 + B * delta_y) / A;
    
    new_x = old_x + delta_x;
    new_y = old_y + delta_y;

    /* Limits, if given */
    if (minx && new_x < VECTOR(*minx)[m]) { new_x = VECTOR(*minx)[m]; }
    if (maxx && new_x > VECTOR(*maxx)[m]) { new_x = VECTOR(*maxx)[m]; }
    if (miny && new_y < VECTOR(*miny)[m]) { new_y = VECTOR(*miny)[m]; }
    if (maxy && new_y > VECTOR(*maxy)[m]) { new_y = VECTOR(*maxy)[m]; }

    /* Update delta, only with/for the affected node */
    VECTOR(D1)[m] = VECTOR(D2)[m] = 0.0;
    for (i=0; i<no_nodes; i++) {
      igraph_real_t old_dx, old_dy, old_mi, new_dx, new_dy, new_mi_dist, old_mi_dist;
      if (i==m) { continue; }
      old_dx=old_x - MATRIX(*res, i, 0);
      old_dy=old_y - MATRIX(*res, i, 1);
      old_mi_dist=sqrt(old_dx * old_dx + old_dy * old_dy);
      new_dx=new_x - MATRIX(*res, i, 0);
      new_dy=new_y - MATRIX(*res, i, 1);
      new_mi_dist=sqrt(new_dx * new_dx + new_dy * new_dy);

      VECTOR(D1)[i] -= MATRIX(kij, m, i) * 
	(-old_dx + MATRIX(lij, m, i) * old_dx / old_mi_dist);
      VECTOR(D2)[i] -= MATRIX(kij, m, i) *
	(-old_dy + MATRIX(lij, m, i) * old_dy / old_mi_dist);
      VECTOR(D1)[i] += MATRIX(kij, m, i) *
	(-new_dx + MATRIX(lij, m, i) * new_dx / new_mi_dist);
      VECTOR(D2)[i] += MATRIX(kij, m, i) *
	(-new_dy + MATRIX(lij, m, i) * new_dy / new_mi_dist);

      VECTOR(D1)[m] += MATRIX(kij, m, i) *
	(new_dx - MATRIX(lij, m, i) * new_dx / new_mi_dist);
      VECTOR(D2)[m] += MATRIX(kij, m, i) *
	(new_dy - MATRIX(lij, m, i) * new_dy / new_mi_dist);
    }
      
    /* Update coordinates*/
    MATRIX(*res, m, 0) = new_x;
    MATRIX(*res, m, 1) = new_y;
  }

  igraph_vector_destroy(&D2);
  igraph_vector_destroy(&D1);
  igraph_matrix_destroy(&lij);
  igraph_matrix_destroy(&kij);
  igraph_matrix_destroy(&dij);
  IGRAPH_FINALLY_CLEAN(5);

  return 0;
}

/**
 * \ingroup layout
 * \function igraph_layout_kamada_kawai_3d
 * \brief 3D version of the Kamada-Kawai layout generator
 *
 * </para><para>
 * This is a force directed layout, see  Kamada, T. and Kawai, S.: An
 * Algorithm for Drawing General Undirected Graphs. Information
 * Processing Letters, 31/1, 7--15, 1989.
 * \param graph A graph object.
 * \param res Pointer to an initialized matrix object. This will
 *        contain the result (x-positions in column zero and
 *        y-positions in column one) and will be resized if needed.
 * \param use_seed Boolean, whether to use the values supplied in the
 *        \p res argument as the initial configuration. If zero then a
 *        random initial configuration is used.
 * \param maxiter The maximum number of iterations to perform. A reasonable
 *        default value is at least ten (or more) times the number of 
 *        vertices.  
 * \param epsilon Stop the iteration, if the maximum delta value of the 
 *        algorithm is smaller than still. It is safe to leave it at zero, 
 *        and then \p maxiter iterations are performed.
 * \param kkconst The Kamada-Kawai vertex attraction constant.
 *        Typical value: number of vertices.
 * \param weights Edge weights, larger values will result longer edges.
 * \param minx Pointer to a vector, or a \c NULL pointer. If not a 
 *        \c NULL pointer then the vector gives the minimum
 *        \quote x \endquote coordinate for every vertex.
 * \param maxx Same as \p minx, but the maximum \quote x \endquote 
 *        coordinates.
 * \param miny Pointer to a vector, or a \c NULL pointer. If not a 
 *        \c NULL pointer then the vector gives the minimum
 *        \quote y \endquote coordinate for every vertex.
 * \param maxy Same as \p miny, but the maximum \quote y \endquote 
 *        coordinates.
 * \param minz Pointer to a vector, or a \c NULL pointer. If not a 
 *        \c NULL pointer then the vector gives the minimum
 *        \quote z \endquote coordinate for every vertex.
 * \param maxz Same as \p minz, but the maximum \quote z \endquote 
 *        coordinates.
 * \return Error code.
 * 
 * Time complexity: O(|V|) for each iteration, after an O(|V|^2
 * log|V|) initialization step. |V| is the number of vertices in the
 * graph.
 */

int igraph_layout_kamada_kawai_3d(const igraph_t *graph, igraph_matrix_t *res,
	       igraph_bool_t use_seed, igraph_integer_t maxiter,
	       igraph_real_t epsilon, igraph_real_t kkconst, 
	       const igraph_vector_t *weights,
	       const igraph_vector_t *minx, const igraph_vector_t *maxx,
	       const igraph_vector_t *miny, const igraph_vector_t *maxy,
	       const igraph_vector_t *minz, const igraph_vector_t *maxz) {
  
  igraph_integer_t no_nodes=igraph_vcount(graph);
  igraph_integer_t no_edges=igraph_ecount(graph);
  igraph_real_t L, L0=sqrt(no_nodes);  
  igraph_matrix_t dij, lij, kij;
  igraph_real_t max_dij;
  igraph_vector_t D1, D2, D3;
  igraph_integer_t i, j, m;

  if (maxiter < 0) {
    IGRAPH_ERROR("Number of iterations must be non-negatice in "
		 "Kamada-Kawai layout", IGRAPH_EINVAL);
  }
  if (kkconst <= 0) {
    IGRAPH_ERROR("`K' constant must be positive in Kamada-Kawai layout",
		 IGRAPH_EINVAL);
  }

  if (use_seed && (igraph_matrix_nrow(res) != no_nodes ||
		   igraph_matrix_ncol(res) != 3)) {
    IGRAPH_ERROR("Invalid start position matrix size in "
		 "3d Kamada-Kawai layout", IGRAPH_EINVAL);
  }
  if (weights && igraph_vector_size(weights) != no_edges) {
    IGRAPH_ERROR("Invalid weight vector length", IGRAPH_EINVAL);
  }

  if (minx && igraph_vector_size(minx) != no_nodes) {
    IGRAPH_ERROR("Invalid minx vector length", IGRAPH_EINVAL);
  }
  if (maxx && igraph_vector_size(maxx) != no_nodes) {
    IGRAPH_ERROR("Invalid maxx vector length", IGRAPH_EINVAL);
  }
  if (minx && maxx && !igraph_vector_all_le(minx, maxx)) {
    IGRAPH_ERROR("minx must not be greater than maxx", IGRAPH_EINVAL);
  }
  if (miny && igraph_vector_size(miny) != no_nodes) {
    IGRAPH_ERROR("Invalid miny vector length", IGRAPH_EINVAL);
  }
  if (maxy && igraph_vector_size(maxy) != no_nodes) {
    IGRAPH_ERROR("Invalid maxy vector length", IGRAPH_EINVAL);
  }
  if (miny && maxy && !igraph_vector_all_le(miny, maxy)) {
    IGRAPH_ERROR("miny must not be greater than maxy", IGRAPH_EINVAL);
  }
  if (minz && igraph_vector_size(minz) != no_nodes) {
    IGRAPH_ERROR("Invalid minz vector length", IGRAPH_EINVAL);
  }
  if (maxz && igraph_vector_size(maxz) != no_nodes) {
    IGRAPH_ERROR("Invalid maxz vector length", IGRAPH_EINVAL);
  }
  if (minz && maxz && !igraph_vector_all_le(minz, maxz)) {
    IGRAPH_ERROR("minz must not be greater than maxz", IGRAPH_EINVAL);
  }

  if (!use_seed) {
    if (minx || maxx || miny || maxy || minz || maxz) {
      const igraph_real_t width=sqrt(no_nodes), height=width, depth=width;
      IGRAPH_CHECK(igraph_matrix_resize(res, no_nodes, 3));
      RNG_BEGIN();
      for (i=0; i<no_nodes; i++) {
	igraph_real_t x1=minx ? VECTOR(*minx)[i] : -width/2;
	igraph_real_t x2=maxx ? VECTOR(*maxx)[i] :  width/2;
	igraph_real_t y1=miny ? VECTOR(*miny)[i] : -height/2;
	igraph_real_t y2=maxy ? VECTOR(*maxy)[i] :  height/2;
	igraph_real_t z1=minz ? VECTOR(*minz)[i] : -depth/2;
	igraph_real_t z2=maxz ? VECTOR(*maxz)[i] :  depth/2;
	if (!igraph_finite(x1)) { x1 = -width/2; }
	if (!igraph_finite(x2)) { x2 =  width/2; }
	if (!igraph_finite(y1)) { y1 = -height/2; }
	if (!igraph_finite(y2)) { y2 =  height/2; }
	if (!igraph_finite(z1)) { z1 = -depth/2; }
	if (!igraph_finite(z2)) { z2 =  depth/2; }
	MATRIX(*res, i, 0) = RNG_UNIF(x1, x2);
	MATRIX(*res, i, 1) = RNG_UNIF(y1, y2);
	MATRIX(*res, i, 2) = RNG_UNIF(z1, z2);
      }
      RNG_END();
    } else {
      igraph_layout_sphere(graph, res);
    }
  }

  if (no_nodes <= 1) { return 0; }

  IGRAPH_MATRIX_INIT_FINALLY(&dij, no_nodes, no_nodes);
  IGRAPH_MATRIX_INIT_FINALLY(&kij, no_nodes, no_nodes);
  IGRAPH_MATRIX_INIT_FINALLY(&lij, no_nodes, no_nodes);
  IGRAPH_CHECK(igraph_shortest_paths_dijkstra(graph, &dij, igraph_vss_all(),
					      igraph_vss_all(), weights,
					      IGRAPH_ALL));
  
  max_dij = 0.0;
  for (i=0; i<no_nodes; i++) {
    for (j=i+1; j<no_nodes; j++) {
      if (!igraph_finite(MATRIX(dij, i, j))) { continue; }
      if (MATRIX(dij, i, j) > max_dij) { max_dij = MATRIX(dij, i, j); }
    }
  }
  for (i=0; i<no_nodes; i++) {
    for (j=0; j<no_nodes; j++) {
      if (MATRIX(dij, i, j) > max_dij) { MATRIX(dij, i, j) = max_dij; }
    }
  }

  L = L0 / max_dij;
  for (i=0; i<no_nodes; i++) {
    for (j=0; j<no_nodes; j++) {      
      igraph_real_t tmp=MATRIX(dij, i, j) * MATRIX(dij, i, j);
      if (i==j) { continue; }
      MATRIX(kij, i, j) = kkconst / tmp;
      MATRIX(lij, i, j) = L * MATRIX(dij, i, j);
    }
  }

  /* Initialize delta */
  IGRAPH_VECTOR_INIT_FINALLY(&D1, no_nodes);
  IGRAPH_VECTOR_INIT_FINALLY(&D2, no_nodes);
  IGRAPH_VECTOR_INIT_FINALLY(&D3, no_nodes);
  for (m=0; m<no_nodes; m++) {
    igraph_real_t dx, dy, dz, mi_dist;
    igraph_real_t myD1=0.0, myD2=0.0, myD3=0.0;
    for (i=0; i<no_nodes; i++) { 
      if (i==m) { continue; }
      dx=MATRIX(*res, m, 0) - MATRIX(*res, i, 0);
      dy=MATRIX(*res, m, 1) - MATRIX(*res, i, 1);
      dz=MATRIX(*res, m, 2) - MATRIX(*res, i, 2);
      mi_dist=sqrt(dx * dx + dy * dy + dz * dz);
      myD1 += MATRIX(kij, m, i) * (dx - MATRIX(lij, m, i) * dx / mi_dist);
      myD2 += MATRIX(kij, m, i) * (dy - MATRIX(lij, m, i) * dy / mi_dist);
      myD3 += MATRIX(kij, m, i) * (dz - MATRIX(lij, m, i) * dz / mi_dist);
    }
    VECTOR(D1)[m] = myD1;
    VECTOR(D2)[m] = myD2;
    VECTOR(D3)[m] = myD3;
  }

  for (j=0; j<maxiter; j++) {
    
    igraph_real_t Ax=0.0, Ay=0.0, Az=0.0;
    igraph_real_t Axx=0.0, Axy=0.0, Axz=0.0, Ayy=0.0, Ayz=0.0, Azz=0.0;
    igraph_real_t max_delta, delta_x, delta_y, delta_z;
    igraph_real_t old_x, old_y, old_z, new_x, new_y, new_z;
    igraph_real_t detnum;

    /* Select maximal delta */
    m=0; max_delta=-1;
    for (i=0; i<no_nodes; i++) {
      igraph_real_t delta=(VECTOR(D1)[i] * VECTOR(D1)[i] + 
			   VECTOR(D2)[i] * VECTOR(D2)[i] +
			   VECTOR(D3)[i] * VECTOR(D3)[i]);
      if (delta > max_delta) { 
	m=i; max_delta=delta;
      }
    }
    if (max_delta < epsilon) { break; }
    old_x=MATRIX(*res, m, 0);
    old_y=MATRIX(*res, m, 1);
    old_z=MATRIX(*res, m, 2);
    
    /* Calculate D1, D2 and D3, and other coefficients */
    for (i=0; i<no_nodes; i++) {
      igraph_real_t dx, dy, dz, dist, den, k_mi, l_mi;
      if (i==m) { continue; }
      dx=old_x - MATRIX(*res, i, 0);
      dy=old_y - MATRIX(*res, i, 1);
      dz=old_z - MATRIX(*res, i, 2);
      dist=sqrt(dx * dx + dy * dy + dz *dz);
      den=dist * (dx * dx + dy * dy + dz * dz);
      k_mi=MATRIX(kij, m, i);
      l_mi=MATRIX(lij, m, i);
      Axx += k_mi * (1 - l_mi * (dy*dy + dz*dz) / den);
      Ayy += k_mi * (1 - l_mi * (dx*dx + dz*dz) / den);
      Azz += k_mi * (1 - l_mi * (dx*dx + dy*dy) / den);
      Axy += k_mi * l_mi * dx * dy / den;
      Axz += k_mi * l_mi * dx * dz / den;
      Ayz += k_mi * l_mi * dy * dz / den;
    }
    Ax = -VECTOR(D1)[m];
    Ay = -VECTOR(D2)[m];
    Az = -VECTOR(D3)[m];

    /* Need to solve some linear equations, we just use Cramer's rule */
#define DET(a,b,c,d,e,f,g,h,i) ((a*e*i+b*f*g+c*d*h)-(c*e*g+b*d*i+a*f*h))
    
    detnum  = DET(Axx,Axy,Axz, Axy,Ayy,Ayz, Axz,Ayz,Azz);
    delta_x = DET(Ax ,Ay ,Az , Axy,Ayy,Ayz, Axz,Ayz,Azz) / detnum;
    delta_y = DET(Axx,Axy,Axz, Ax ,Ay ,Az , Axz,Ayz,Azz) / detnum;
    delta_z = DET(Axx,Axy,Axz, Axy,Ayy,Ayz, Ax ,Ay ,Az ) / detnum;
    
    new_x = old_x + delta_x;
    new_y = old_y + delta_y;
    new_z = old_z + delta_z;

    /* Limits, if given */
    if (minx && new_x < VECTOR(*minx)[m]) { new_x = VECTOR(*minx)[m]; }
    if (maxx && new_x > VECTOR(*maxx)[m]) { new_x = VECTOR(*maxx)[m]; }
    if (miny && new_y < VECTOR(*miny)[m]) { new_y = VECTOR(*miny)[m]; }
    if (maxy && new_y > VECTOR(*maxy)[m]) { new_y = VECTOR(*maxy)[m]; }
    if (minz && new_z < VECTOR(*minz)[m]) { new_z = VECTOR(*minz)[m]; }
    if (maxz && new_z > VECTOR(*maxz)[m]) { new_z = VECTOR(*maxz)[m]; }

    /* Update delta, only with/for the affected node */
    VECTOR(D1)[m] = VECTOR(D2)[m] = VECTOR(D3)[m] = 0.0;
    for (i=0; i<no_nodes; i++) {
      igraph_real_t old_dx, old_dy, old_dz, old_mi_dist, new_dx, new_dy, new_dz, new_mi_dist;
      if (i==m) { continue; }
      old_dx=old_x - MATRIX(*res, i, 0);
      old_dy=old_y - MATRIX(*res, i, 1);
      old_dz=old_z - MATRIX(*res, i, 2);
      old_mi_dist=sqrt(old_dx * old_dx + old_dy * old_dy +
				     old_dz * old_dz);
      new_dx=new_x - MATRIX(*res, i, 0);
      new_dy=new_y - MATRIX(*res, i, 1);
      new_dz=new_z - MATRIX(*res, i, 2);
      new_mi_dist=sqrt(new_dx * new_dx + new_dy * new_dy +
				     new_dz * new_dz);

      VECTOR(D1)[i] -= MATRIX(kij, m, i) * 
	(-old_dx + MATRIX(lij, m, i) * old_dx / old_mi_dist);
      VECTOR(D2)[i] -= MATRIX(kij, m, i) *
	(-old_dy + MATRIX(lij, m, i) * old_dy / old_mi_dist);
      VECTOR(D3)[i] -= MATRIX(kij, m, i) *
	(-old_dz + MATRIX(lij, m, i) * old_dz / old_mi_dist);

      VECTOR(D1)[i] += MATRIX(kij, m, i) *
	(-new_dx + MATRIX(lij, m, i) * new_dx / new_mi_dist);
      VECTOR(D2)[i] += MATRIX(kij, m, i) *
	(-new_dy + MATRIX(lij, m, i) * new_dy / new_mi_dist);
      VECTOR(D3)[i] += MATRIX(kij, m, i) *
	(-new_dz + MATRIX(lij, m, i) * new_dz / new_mi_dist);

      VECTOR(D1)[m] += MATRIX(kij, m, i) *
	(new_dx - MATRIX(lij, m, i) * new_dx / new_mi_dist);
      VECTOR(D2)[m] += MATRIX(kij, m, i) *
	(new_dy - MATRIX(lij, m, i) * new_dy / new_mi_dist);
      VECTOR(D3)[m] += MATRIX(kij, m, i) *
	(new_dz - MATRIX(lij, m, i) * new_dz / new_mi_dist);
    }
      
    /* Update coordinates*/
    MATRIX(*res, m, 0) = new_x;
    MATRIX(*res, m, 1) = new_y;
    MATRIX(*res, m, 2) = new_z;
  }

  igraph_vector_destroy(&D3);
  igraph_vector_destroy(&D2);
  igraph_vector_destroy(&D1);
  igraph_matrix_destroy(&lij);
  igraph_matrix_destroy(&kij);
  igraph_matrix_destroy(&dij);
  IGRAPH_FINALLY_CLEAN(6);

  return 0;
}
