/*************************************************************************
 * 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: Details at https://graphviz.org
 *************************************************************************/

#include "config.h"
#include <neatogen/overlap.h>

#if ((defined(HAVE_GTS) || defined(HAVE_TRIANGLE)) && defined(SFDP))

#include <sparse/SparseMatrix.h>
#include <neatogen/call_tri.h>
#include <rbtree/red_black_tree.h>
#include <common/types.h>
#include <math.h>
#include <common/memory.h>
#include <common/globals.h>
#include <time.h>

static void ideal_distance_avoid_overlap(int dim, SparseMatrix A, double *x, double *width, double *ideal_distance, double *tmax, double *tmin){
  /*  if (x1>x2 && y1 > y2) we want either x1 + t (x1-x2) - x2 > (width1+width2), or y1 + t (y1-y2) - y2 > (height1+height2),
      hence t = MAX(expandmin, MIN(expandmax, (width1+width2)/(x1-x2) - 1, (height1+height2)/(y1-y2) - 1)), and
      new ideal distance = (1+t) old_distance. t can be negative sometimes.
      The result ideal distance is set to negative if the edge needs shrinking
  */
  int i, j, jj;
  int *ia = A->ia, *ja = A->ja;
  double dist, dx, dy, wx, wy, t;
  double expandmax = 1.5, expandmin = 1;

  *tmax = 0;
  *tmin = 1.e10;
  assert(SparseMatrix_is_symmetric(A, FALSE));
  for (i = 0; i < A->m; i++){
    for (j = ia[i]; j < ia[i+1]; j++){
      jj = ja[j];
      if (jj == i) continue;
      dist = distance(x, dim, i, jj);
      dx = fabs(x[i*dim] - x[jj*dim]);
      dy = fabs(x[i*dim+1] - x[jj*dim+1]);
      wx = width[i*dim]+width[jj*dim];
      wy = width[i*dim+1]+width[jj*dim+1];
      if (dx < MACHINEACC*wx && dy < MACHINEACC*wy){
	ideal_distance[j] = sqrt(wx*wx+wy*wy);
	*tmax = 2;
      } else {
	if (dx < MACHINEACC*wx){
	  t = wy/dy;
	} else if (dy < MACHINEACC*wy){
	  t = wx/dx;
	} else {
	  t = MIN(wx/dx, wy/dy);
	}
	if (t > 1) t = MAX(t, 1.001);/* no point in things like t = 1.00000001 as this slow down convergence */
	*tmax = MAX(*tmax, t);
	*tmin = MIN(*tmin, t);
	t = MIN(expandmax, t);
	t = MAX(expandmin, t);
	if (t > 1) {
	  ideal_distance[j] = t*dist;
	} else {
	  ideal_distance[j] = -t*dist;
	}
      }

    }
  }
  return;
}

enum {INTV_OPEN, INTV_CLOSE};

struct scan_point_struct{
  int node;
  double x;
  int status;
};

typedef struct scan_point_struct scan_point;


static int comp_scan_points(const void *p, const void *q){
  const scan_point *pp = p;
  const scan_point *qq = q;
  if (pp->x > qq->x){
    return 1;
  } else if (pp->x < qq->x){
    return -1;
  } else {
    if (pp->node > qq->node){
      return 1;
    } else if (pp->node < qq->node){
      return -1;
    }
    return 0;
  }
  return 0;
}

static void NodeDest(void* a) {
  /*  free((int*)a);*/
}

static int NodeComp(const void* a,const void* b) {
  return comp_scan_points(a,b);

}

static void NodePrint(const void* a) {
  const scan_point *aa = a;
  fprintf(stderr, "node {%d, %f, %d}\n", aa->node, aa->x, aa->status);
}

static void InfoPrint(void* a) {
}

static void InfoDest(void *a){
}

static SparseMatrix get_overlap_graph(int dim, int n, double *x, double *width, int check_overlap_only){
  /* if check_overlap_only = TRUE, we only check whether there is one overlap */
  scan_point *scanpointsx, *scanpointsy;
  int i, k, neighbor;
  SparseMatrix A = NULL, B = NULL;
  rb_red_blk_node *newNode, *newNode0, *newNode2 = NULL;
  rb_red_blk_tree* treey;
  double one = 1;

  A = SparseMatrix_new(n, n, 1, MATRIX_TYPE_REAL, FORMAT_COORD);

  scanpointsx = N_GNEW(2*n,scan_point);
  for (i = 0; i < n; i++){
    scanpointsx[2*i].node = i;
    scanpointsx[2*i].x = x[i*dim] - width[i*dim];
    scanpointsx[2*i].status = INTV_OPEN;
    scanpointsx[2*i+1].node = i+n;
    scanpointsx[2*i+1].x = x[i*dim] + width[i*dim];
    scanpointsx[2*i+1].status = INTV_CLOSE;
  }
  qsort(scanpointsx, 2*n, sizeof(scan_point), comp_scan_points);

  scanpointsy = N_GNEW(2*n,scan_point);
  for (i = 0; i < n; i++){
    scanpointsy[i].node = i;
    scanpointsy[i].x = x[i*dim+1] - width[i*dim+1];
    scanpointsy[i].status = INTV_OPEN;
    scanpointsy[i+n].node = i;
    scanpointsy[i+n].x = x[i*dim+1] + width[i*dim+1];
    scanpointsy[i+n].status = INTV_CLOSE;
  }


  treey = RBTreeCreate(NodeComp,NodeDest,InfoDest,NodePrint,InfoPrint);

  for (i = 0; i < 2*n; i++){
#ifdef DEBUG_RBTREE
    fprintf(stderr," k = %d node = %d x====%f\n",(scanpointsx[i].node)%n, (scanpointsx[i].node), (scanpointsx[i].x));
#endif

    k = (scanpointsx[i].node)%n;


    if (scanpointsx[i].status == INTV_OPEN){
#ifdef DEBUG_RBTREE
      fprintf(stderr, "inserting...");
      treey->PrintKey(&(scanpointsy[k]));
#endif

      RBTreeInsert(treey, &(scanpointsy[k]), NULL); /* add both open and close int for y */

#ifdef DEBUG_RBTREE
      fprintf(stderr, "inserting2...");
      treey->PrintKey(&(scanpointsy[k+n]));
#endif

      RBTreeInsert(treey, &(scanpointsy[k+n]), NULL);
    } else {
      double bsta, bbsta, bsto, bbsto; int ii; 

      assert(scanpointsx[i].node >= n);

      newNode = newNode0 = RBExactQuery(treey, &(scanpointsy[k + n]));
      ii = ((scan_point *)newNode->key)->node;
      assert(ii < n);
      bsta = scanpointsy[ii].x; bsto = scanpointsy[ii+n].x;

#ifdef DEBUG_RBTREE
      fprintf(stderr, "poping..%d....yinterval={%f,%f}\n", scanpointsy[k + n].node, bsta, bsto);
      treey->PrintKey(newNode->key);
#endif

     assert(treey->nil != newNode);
      while ((newNode) && ((newNode = TreePredecessor(treey, newNode)) != treey->nil)){
	neighbor = (((scan_point *)newNode->key)->node)%n;
	bbsta = scanpointsy[neighbor].x; bbsto = scanpointsy[neighbor+n].x;/* the y-interval of the node that has one end of the interval lower than the top of the leaving interval (bsto) */
#ifdef DEBUG_RBTREE
	fprintf(stderr," predecessor is node %d y = %f\n", ((scan_point *)newNode->key)->node, ((scan_point *)newNode->key)->x);
#endif
	if (neighbor != k){
	  if (fabs(0.5*(bsta+bsto) - 0.5*(bbsta+bbsto)) < 0.5*(bsto-bsta) + 0.5*(bbsto-bbsta)){/* if the distance of the centers of the interval is less than sum of width, we have overlap */
	    A = SparseMatrix_coordinate_form_add_entries(A, 1, &neighbor, &k, &one);
#ifdef DEBUG_RBTREE
	    fprintf(stderr,"======================================  %d %d\n",k,neighbor);
#endif
	    if (check_overlap_only) goto check_overlap_RETURN;
	  }
	} else {
	  newNode2 = newNode;
	}

      }

#ifdef DEBUG_RBTREE
      fprintf(stderr, "deleteing...");
      treey->PrintKey(newNode0->key);
#endif

      if (newNode0) RBDelete(treey,newNode0);


     if (newNode2 && newNode2 != treey->nil && newNode2 != newNode0) {

#ifdef DEBUG_RBTREE
	fprintf(stderr, "deleteing2...");
	treey->PrintKey(newNode2->key);
#endif

	if (newNode0) RBDelete(treey,newNode2);
      }

    }
  }

check_overlap_RETURN:
   free(scanpointsx);
  free(scanpointsy);
  RBTreeDestroy(treey);

  B = SparseMatrix_from_coordinate_format(A);
  SparseMatrix_delete(A);
  A = SparseMatrix_symmetrize(B, FALSE);
  SparseMatrix_delete(B);
  if (Verbose) fprintf(stderr, "found %d clashes\n", A->nz);
  return A;
}



/* ============================== label overlap smoother ==================*/


static void relative_position_constraints_delete(void *d){
  relative_position_constraints data;
  if (!d) return;
  data = (relative_position_constraints) d;
  free(data->irn);
  free(data->jcn);
  free(data->val);
  /* other stuff inside relative_position_constraints is assed back to the user hence no need to deallocator*/
  free(d);
}

static relative_position_constraints relative_position_constraints_new(SparseMatrix A_constr, int edge_labeling_scheme, int n_constr_nodes, int *constr_nodes){
    relative_position_constraints data;
    assert(A_constr);
    data = MALLOC(sizeof(struct relative_position_constraints_struct));
    data->constr_penalty = 1;
    data->edge_labeling_scheme = edge_labeling_scheme;
    data->n_constr_nodes = n_constr_nodes;
    data->constr_nodes = constr_nodes;
    data->A_constr = A_constr;
    data->irn = NULL;
    data->jcn = NULL;
    data->val = NULL;

    return data;
}
static void scale_coord(int dim, int m, double *x, double scale){
  int i;
  for (i = 0; i < dim*m; i++) {
    x[i] *= scale;
  }
}

double overlap_scaling(int dim, int m, double *x, double *width, double scale_sta, double scale_sto, double epsilon, int maxiter){
  /* do a bisection between scale_sta and scale_sto, up to maxiter iterations or till interval <= epsilon, to find the best scaling to avoid overlap
     m: number of points
     x: the coordinates
     width: label size
     scale_sta: starting bracket. If <= 0, assumed 0. If > 0, we will test this first and if no overlap, return.
     scale_sto: stopping bracket. This must be overlap free if positive. If <= 0, we will find automatically by doubling from scale_sta, or epsilon if scale_sta <= 0.
     typically usage: 
     - for shrinking down a layout to reduce white space, we will assume scale_sta and scale_sto are both given and positive, and scale_sta is the current guess.
     - for scaling up, we assume scale_sta, scale_sto <= 0
   */
  double scale = -1, scale_best = -1;
  SparseMatrix C = NULL;
  int check_overlap_only = 1;
  int overlap = 0;
  double two = 2;
  int iter = 0;

  assert(epsilon > 0);

  if (scale_sta <= 0) {
    scale_sta = 0;
  } else {
    scale_coord(dim, m, x, scale_sta);
    C = get_overlap_graph(dim, m, x, width, check_overlap_only);
    if (!C || C->nz == 0) {
      if (Verbose) fprintf(stderr," shrinking with %f works\n", scale_sta);
      SparseMatrix_delete(C);
      return scale_sta;
    }
    scale_coord(dim, m, x, 1./scale_sta);
    SparseMatrix_delete(C);
  }

  if (scale_sto < 0){
    if (scale_sta == 0) {
      scale_sto = epsilon;
    } else {
      scale_sto = scale_sta;
    }
    scale_coord(dim, m, x, scale_sto);
    do {
      scale_sto *= two;
      scale_coord(dim, m, x, two);
      C = get_overlap_graph(dim, m, x, width, check_overlap_only);
      overlap = (C && C->nz > 0);
      SparseMatrix_delete(C);
    } while (overlap);
    scale_coord(dim, m, x, 1/scale_sto);/* unscale */
  }

  scale_best = scale_sto;
  while (iter++ < maxiter && scale_sto - scale_sta > epsilon){

    if (Verbose) fprintf(stderr,"in overlap_scaling iter=%d, maxiter=%d, scaling bracket: {%f,%f}\n", iter, maxiter, scale_sta, scale_sto);

    scale = 0.5*(scale_sta + scale_sto);
    scale_coord(dim, m, x, scale);
    C = get_overlap_graph(dim, m, x, width, check_overlap_only);
    scale_coord(dim, m, x, 1./scale);/* unscale */
    overlap = (C && C->nz > 0);
    SparseMatrix_delete(C);
    if (overlap){
      scale_sta = scale;
    } else {
      scale_best = scale_sto = scale;
    }
  }

  /* final scaling */
  scale_coord(dim, m, x, scale_best);
  return scale_best;
}
 
OverlapSmoother OverlapSmoother_new(SparseMatrix A, int m, 
				    int dim, double lambda0, double *x, double *width, int include_original_graph, int neighborhood_only, 
				    double *max_overlap, double *min_overlap,
				    int edge_labeling_scheme, int n_constr_nodes, int *constr_nodes, SparseMatrix A_constr, int shrink
				    ){
  OverlapSmoother sm;
  int i, j, k, *iw, *jw, jdiag;
  SparseMatrix B;
  double *lambda, *d, *w, diag_d, diag_w, dist;

  assert((!A) || SparseMatrix_is_symmetric(A, FALSE));

  sm = GNEW(struct OverlapSmoother_struct);
  sm->scheme = SM_SCHEME_NORMAL;
  if (constr_nodes && n_constr_nodes > 0 && edge_labeling_scheme != ELSCHEME_NONE){
    sm->scheme = SM_SCHEME_NORMAL_ELABEL;
    sm->data = relative_position_constraints_new(A_constr, edge_labeling_scheme, n_constr_nodes, constr_nodes);
    sm->data_deallocator = relative_position_constraints_delete;
  } else {
    sm->data = NULL;
  }

  sm->tol_cg = 0.01;
  sm->maxit_cg = sqrt((double) A->m);

  lambda = sm->lambda = N_GNEW(m,double);
  for (i = 0; i < m; i++) sm->lambda[i] = lambda0;
  
  B= call_tri(m, dim, x);

  if (!neighborhood_only){
    SparseMatrix C, D;
    C = get_overlap_graph(dim, m, x, width, 0);
    D = SparseMatrix_add(B, C);
    SparseMatrix_delete(B);
    SparseMatrix_delete(C);
    B = D;
  }
  if (include_original_graph){
    sm->Lw = SparseMatrix_add(A, B);
    SparseMatrix_delete(B);
  } else {
    sm->Lw = B;
  }
  sm->Lwd = SparseMatrix_copy(sm->Lw);

#ifdef DEBUG
  {
    FILE *fp;
    fp = fopen("/tmp/111","w");
    export_embedding(fp, dim, sm->Lwd, x, NULL);
    fclose(fp);
  }
#endif

  if (!(sm->Lw) || !(sm->Lwd)) {
    OverlapSmoother_delete(sm);
    return NULL;
  }

  assert((sm->Lwd)->type == MATRIX_TYPE_REAL);
  
  ideal_distance_avoid_overlap(dim, sm->Lwd, x, width, (double*) (sm->Lwd->a), max_overlap, min_overlap);

  /* no overlap at all! */
  if (*max_overlap < 1 && shrink){
    double scale_sta = MIN(1, *max_overlap*1.0001), scale_sto = 1;

    if (Verbose) fprintf(stderr," no overlap (overlap = %f), rescale to shrink\n", *max_overlap - 1);

    scale_sta = overlap_scaling(dim, m, x, width, scale_sta, scale_sto, 0.0001, 15);

    *max_overlap = 1;
    goto RETURN;
  }

  iw = sm->Lw->ia; jw = sm->Lw->ja;
  w = (double*) sm->Lw->a; d = (double*) 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;
      }
      if (d[j] > 0){/* those edges that needs expansion */
	w[j] = -100/d[j]/d[j];
      } else {/* those that needs shrinking is set to negative in ideal_distance_avoid_overlap */
	w[j] = -1/d[j]/d[j];
	d[j] = -d[j];
      }
      dist = d[j];
      diag_w += w[j];
      d[j] = w[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;
  }
 RETURN:
  return sm;
}

void OverlapSmoother_delete(OverlapSmoother sm){

  StressMajorizationSmoother_delete(sm);

}

double OverlapSmoother_smooth(OverlapSmoother sm, int dim, double *x){
  int maxit_sm = 1;/* only using 1 iteration of stress majorization 
		      is found to give better results and save time! */
  double res = StressMajorizationSmoother_smooth(sm, dim, x, maxit_sm, 0.001);
#ifdef DEBUG
  {FILE *fp;
  fp = fopen("/tmp/222","w");
  export_embedding(fp, dim, sm->Lwd, x, NULL);
  fclose(fp);}
#endif
  return res;
}

/*================================= end OverlapSmoother =============*/

static void scale_to_edge_length(int dim, SparseMatrix A, double *x, double avg_label_size){
  double dist;
  int i;

  if (!A) return;
  dist = average_edge_length(A, dim, x);
  if (Verbose) fprintf(stderr,"avg edge len=%f avg_label-size= %f\n", dist, avg_label_size);


  dist = avg_label_size/MAX(dist, MACHINEACC);

  for (i = 0; i < dim*A->m; i++) x[i] *= dist;
}

static void print_bounding_box(int n, int dim, double *x){
  double *xmin, *xmax;
  int i, k;

  xmin = N_GNEW(dim,double);
  xmax = N_GNEW(dim,double);

  for (i = 0; i < dim; i++) xmin[i]=xmax[i] = x[i];

  for (i = 0; i < n; i++){
    for (k = 0; k < dim; k++){
      xmin[k] = MIN(xmin[k],x[i*dim+k]);
      xmax[k] = MAX(xmax[k],x[i*dim+k]);
    }
  }
  fprintf(stderr,"bounding box = \n");
  for (i = 0; i < dim; i++) fprintf(stderr,"{%f,%f}, ",xmin[i], xmax[i]);
  fprintf(stderr,"\n");

  free(xmin);
  free(xmax);
}

static int check_convergence(double max_overlap, double res, int has_penalty_terms, double epsilon){
  if (!has_penalty_terms) return (max_overlap <= 1);
  return res < epsilon;
}

void remove_overlap(int dim, SparseMatrix A, double *x, double *label_sizes, int ntry, double initial_scaling, 
		    int edge_labeling_scheme, int n_constr_nodes, int *constr_nodes, SparseMatrix A_constr, int do_shrinking){
  /* 
     edge_labeling_scheme: if ELSCHEME_NONE, n_constr_nodes/constr_nodes/A_constr are not used

     n_constr_nodes: number of nodes that has constraints, these are nodes that is
     .               constrained to be close to the average of its neighbors.
     constr_nodes: a list of nodes that need to be constrained. If NULL, unused.
     A_constr: neighbors of node i are in the row i of this matrix. i needs to sit
     .         in between these neighbors as much as possible. this must not be NULL
     .         if constr_nodes != NULL.

  */

  double lambda = 0.00;
  OverlapSmoother sm;
  int include_original_graph = 0, i;
  double LARGE = 100000;
  double avg_label_size, res = LARGE;
  double max_overlap = 0, min_overlap = 999;
  int neighborhood_only = TRUE;
  int has_penalty_terms = FALSE;
  double epsilon = 0.005;
  int shrink = 0;

#ifdef TIME
  clock_t  cpu;
#endif

#ifdef TIME
  cpu = clock();
#endif

  if (!label_sizes) return;

  if (initial_scaling < 0) {
    avg_label_size = 0;
    for (i = 0; i < A->m; i++) avg_label_size += label_sizes[i*dim]+label_sizes[i*dim+1];
    avg_label_size /= A->m;
    scale_to_edge_length(dim, A, x, -initial_scaling*avg_label_size);
  } else if (initial_scaling > 0){
    scale_to_edge_length(dim, A, x, initial_scaling);
  }

  if (!ntry) return;

#ifdef DEBUG
  _statistics[0] = _statistics[1] = 0.;
  {FILE*fp;
  fp = fopen("x1","w");
  for (i = 0; i < A->m; i++){
    fprintf(fp, "%f %f\n",x[i*2],x[i*2+1]);
  }
  fclose(fp);
  }
#endif

#ifdef ANIMATE
  {FILE*fp;
    fp = fopen("/tmp/m","wa");
    fprintf(fp,"{");
#endif

  has_penalty_terms = (edge_labeling_scheme != ELSCHEME_NONE && n_constr_nodes > 0);
  for (i = 0; i < ntry; i++){
    if (Verbose) print_bounding_box(A->m, dim, x);
    sm = OverlapSmoother_new(A, A->m, dim, lambda, x, label_sizes, include_original_graph, neighborhood_only,
			     &max_overlap, &min_overlap, edge_labeling_scheme, n_constr_nodes, constr_nodes, A_constr, shrink); 
    if (Verbose) fprintf(stderr, "overlap removal neighbors only?= %d iter -- %d, overlap factor = %g underlap factor = %g\n", neighborhood_only, i, max_overlap - 1, min_overlap);
    if (check_convergence(max_overlap, res, has_penalty_terms, epsilon)){
    
      OverlapSmoother_delete(sm);
      if (neighborhood_only == FALSE){
	break;
      } else {
	res = LARGE;
	neighborhood_only = FALSE; if (do_shrinking) shrink = 1;
	continue;
      }
    }
    
    res = OverlapSmoother_smooth(sm, dim, x);
    if (Verbose) fprintf(stderr,"res = %f\n",res);
#ifdef ANIMATE
    if (i != 0) fprintf(fp,",");
    export_embedding(fp, dim, A, x, label_sizes);
#endif
    OverlapSmoother_delete(sm);
  }
  if (Verbose) fprintf(stderr, "overlap removal neighbors only?= %d iter -- %d, overlap factor = %g underlap factor = %g\n", neighborhood_only, i, max_overlap - 1, min_overlap);

#ifdef ANIMATE
  fprintf(fp,"}");
    fclose(fp);
  }
#endif

  if (has_penalty_terms){
    /* now do without penalty */
    remove_overlap(dim, A, x, label_sizes, ntry, 0.,
		   ELSCHEME_NONE, 0, NULL, NULL, do_shrinking);
  }

#ifdef DEBUG
  fprintf(stderr," number of cg iter = %f, number of stress majorization iter = %f number of overlap removal try = %d\n",
	  _statistics[0], _statistics[1], i - 1);

  {FILE*fp;
  fp = fopen("x2","w");
  for (i = 0; i < A->m; i++){
    fprintf(fp, "%f %f\n",x[i*2],x[i*2+1]);
  }
  fclose(fp);
  }
#endif

#ifdef DEBUG
  {FILE*fp;
  fp = fopen("/tmp/m","w");
  if (A) export_embedding(fp, dim, A, x, label_sizes);
  fclose(fp);
  }
#endif
#ifdef TIME
  fprintf(stderr, "post processing %f\n",((double) (clock() - cpu)) / CLOCKS_PER_SEC);
#endif
}

#else
#include <common/types.h>
#include <sparse/SparseMatrix.h>
void remove_overlap(int dim, SparseMatrix A, double *x, double *label_sizes, int ntry, double initial_scaling,
		    int edge_labeling_scheme, int n_constr_nodes, int *constr_nodes, SparseMatrix A_constr, int do_shrinking)
{
    static int once;

    (void)dim;
    (void)A;
    (void)x;
    (void)label_sizes;
    (void)ntry;
    (void)initial_scaling;
    (void)edge_labeling_scheme;
    (void)n_constr_nodes;
    (void)constr_nodes;
    (void)A_constr;
    (void)do_shrinking;

    if (once == 0) {
	once = 1;
	agerr(AGERR, "remove_overlap: Graphviz not built with triangulation library\n");
    }
}
#endif
