/* -*- mode: C -*-  */
/* 
   IGraph R package.
   Copyright (C) 2006  Gabor Csardi <csardi@rmki.kfki.hu>
   MTA RMKI, Konkoly-Thege Miklos st. 29-33, Budapest 1121, Hungary
   
   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.h"
#include "memory.h"

#include <math.h>

/* internal function */

inline int igraph_2dgrid_which(igraph_2dgrid_t *grid, igraph_real_t xc, igraph_real_t yc,
			long int *x, long int *y) {

  if (xc <= grid->minx) { 
    *x=0; 
  } else if (xc >= grid->maxx) {
    *x=grid->stepsx-1;
  } else {
    *x=floor((xc-(grid->minx))/(grid->deltax));
  }

  if (yc <= grid->miny) {
    *y=0;
  } else if (yc >= grid->maxy) {
    *y=grid->stepsy-1;
  } else {
    *y=floor((yc-(grid->miny))/(grid->deltay));
  }

  return 0;
}

int igraph_2dgrid_init(igraph_2dgrid_t *grid, igraph_matrix_t *coords, 
		       igraph_real_t minx, igraph_real_t maxx, igraph_real_t deltax,
		       igraph_real_t miny, igraph_real_t maxy, igraph_real_t deltay) {
  long int i;
  
  grid->coords=coords;
  grid->minx=minx;
  grid->maxx=maxx;
  grid->deltax=deltax;
  grid->miny=miny;
  grid->maxy=maxy;
  grid->deltay=deltay;
  
  grid->stepsx=ceil((maxx-minx)/deltax);
  grid->stepsy=ceil((maxy-miny)/deltay);

  IGRAPH_CHECK(igraph_matrix_init(&grid->startidx, 
				  grid->stepsx, grid->stepsy));
  IGRAPH_FINALLY(igraph_matrix_destroy, &grid->startidx);
  IGRAPH_VECTOR_INIT_FINALLY(&grid->next, igraph_matrix_nrow(coords));
  IGRAPH_VECTOR_INIT_FINALLY(&grid->prev, igraph_matrix_nrow(coords));

  for (i=0; i<igraph_vector_size(&grid->next); i++) {
    VECTOR(grid->next)[i]=-1;
  }

  grid->massx=0;
  grid->massy=0;
  grid->vertices=0;
  
  IGRAPH_FINALLY_CLEAN(3);
  return 0;
}

void igraph_2dgrid_destroy(igraph_2dgrid_t *grid) {
  igraph_matrix_destroy(&grid->startidx);
  igraph_vector_destroy(&grid->next);
  igraph_vector_destroy(&grid->prev);
}

void igraph_2dgrid_add(igraph_2dgrid_t *grid, long int elem,
		       igraph_real_t xc, igraph_real_t yc) {
  long int x, y;
  long int first;

  MATRIX(*grid->coords, elem, 0)=xc;
  MATRIX(*grid->coords, elem, 1)=yc;

  /* add to cell */
  igraph_2dgrid_which(grid, xc, yc, &x, &y);
  first=MATRIX(grid->startidx, x, y);
  VECTOR(grid->prev)[elem]=0;
  VECTOR(grid->next)[elem]=first;
  if (first != 0) {
    VECTOR(grid->prev)[first-1]=elem+1;
  }
  MATRIX(grid->startidx, x, y)=elem+1;

  grid->massx += xc;
  grid->massy += yc;
  grid->vertices += 1;
}

void igraph_2dgrid_add2(igraph_2dgrid_t *grid, long int elem) {
  long int x, y;
  long int first;
  igraph_real_t xc, yc;

  xc=MATRIX(*grid->coords, elem, 0);
  yc=MATRIX(*grid->coords, elem, 1);

  /* add to cell */
  igraph_2dgrid_which(grid, xc, yc, &x, &y);
  first=MATRIX(grid->startidx, x, y);
  VECTOR(grid->prev)[elem]=0;
  VECTOR(grid->next)[elem]=first;
  if (first != 0) {
    VECTOR(grid->prev)[first-1]=elem+1;
  }
  MATRIX(grid->startidx, x, y)=elem+1;

  grid->massx += xc;
  grid->massy += yc;
  grid->vertices += 1;
}

void igraph_2dgrid_move(igraph_2dgrid_t *grid, long int elem, 
			igraph_real_t xc, igraph_real_t yc) {
  long int oldx, oldy;
  long int newx, newy;
  igraph_real_t oldxc=MATRIX(*grid->coords, elem, 0);
  igraph_real_t oldyc=MATRIX(*grid->coords, elem, 1);
  long int first;

  xc=oldxc+xc; yc=oldyc+yc;

  igraph_2dgrid_which(grid, oldxc, oldyc, &oldx, &oldy);
  igraph_2dgrid_which(grid, xc, yc, &newx, &newy);
  if (oldx != newx || oldy != newy) {
    /* remove from this cell */
    if (VECTOR(grid->prev)[elem] != 0) {
      VECTOR(grid->next) [ (long int) VECTOR(grid->prev)[elem]-1 ] =
	VECTOR(grid->next)[elem];
    } else {
      MATRIX(grid->startidx, oldx, oldy)=VECTOR(grid->next)[elem];
    }
    if (VECTOR(grid->next)[elem] != 0) {
      VECTOR(grid->prev)[ (long int) VECTOR(grid->next)[elem]-1 ] =
	VECTOR(grid->prev)[elem];
    }
    
    /* add to this cell */
    first=MATRIX(grid->startidx, newx, newy);
    VECTOR(grid->prev)[elem]=0;
    VECTOR(grid->next)[elem]=first;
    if (first != 0) {
      VECTOR(grid->prev)[first-1]=elem+1;
    }
    MATRIX(grid->startidx, newx, newy)=elem+1;
  }
  
  grid->massx += -oldxc+xc;
  grid->massy += -oldyc+yc;

  MATRIX(*grid->coords, elem, 0)=xc;
  MATRIX(*grid->coords, elem, 1)=yc;
  
}

void igraph_2dgrid_getcenter(const igraph_2dgrid_t *grid, 
			     igraph_real_t *massx, igraph_real_t *massy) {
  *massx = (grid->massx)/(grid->vertices);
  *massy = (grid->massy)/(grid->vertices);
}

igraph_bool_t igraph_2dgrid_in(const igraph_2dgrid_t *grid, long int elem) {
  return VECTOR(grid->next)[elem] != -1;
}

inline igraph_real_t igraph_2dgrid_dist(const igraph_2dgrid_t *grid, 
			  long int e1, long int e2) {
  igraph_real_t x=MATRIX(*grid->coords, e1, 0)-MATRIX(*grid->coords, e2, 0);
  igraph_real_t y=MATRIX(*grid->coords, e1, 1)-MATRIX(*grid->coords, e2, 1);
  
  return sqrt(x*x + y*y);
}

inline igraph_real_t igraph_2dgrid_dist2(const igraph_2dgrid_t *grid, 
			  long int e1, long int e2) {
  igraph_real_t x=MATRIX(*grid->coords, e1, 0)-MATRIX(*grid->coords, e2, 0);
  igraph_real_t y=MATRIX(*grid->coords, e1, 1)-MATRIX(*grid->coords, e2, 1);
  
  return x*x + y*y;
}

int igraph_i_2dgrid_addvertices(igraph_2dgrid_t *grid, igraph_vector_t *eids,
				igraph_integer_t vid, igraph_real_t r, 
				long int x, long int y) {
  long int act;
  igraph_real_t *v=VECTOR(grid->next);

  r=r*r;
  act=MATRIX(grid->startidx, x, y);
  while (act != 0) {
    if (igraph_2dgrid_dist2(grid, vid, act-1) < r) {
      IGRAPH_CHECK(igraph_vector_push_back(eids, act-1));
    }
    act=v[act-1];
  }
  return 0;
}

int igraph_2dgrid_neighbors(igraph_2dgrid_t *grid, igraph_vector_t *eids, 
			    igraph_integer_t vid, igraph_real_t r) {
  igraph_real_t xc=MATRIX(*grid->coords, (long int)vid, 0);
  igraph_real_t yc=MATRIX(*grid->coords, (long int)vid, 1);
  long int x, y;
  igraph_vector_clear(eids);
  
  igraph_2dgrid_which(grid, xc, yc, &x, &y);

  /* this cell */
  igraph_i_2dgrid_addvertices(grid, eids, vid, r, x, y);
  
  /* left */
  if (x!=0) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x-1, y);
  }
  /* right */
  if (x!=grid->stepsx-1) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x+1, y);
  }
  /* up */
  if (y!=0) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x, y-1);
  }
  /* down */
  if (y!=grid->stepsy-1) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x, y+1);
  }
  /* up & left */
  if (x != 0 && y != 0) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x-1, y-1);
  }
  /* up & right */
  if (x != grid->stepsx-1 && y != 0) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x+1, y-1);
  }
  /* down & left */
  if (x != 0 && y != grid->stepsy-1) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x-1, y+1);
  }
  /* down & right */
  if (x != grid->stepsx-1 && y != grid->stepsy-1) {
    igraph_i_2dgrid_addvertices(grid, eids, vid, r, x-1, y+1);
  }
  
  return 0;
}

void igraph_2dgrid_reset(igraph_2dgrid_t *grid, igraph_2dgrid_iterator_t *it) {
  /* Search for the first cell containing a vertex */
  it->x=0; it->y=0; it->vid=MATRIX(grid->startidx, 0, 0);
  while ( it->vid==0 && (it->x < grid->stepsx-1 || it->y<grid->stepsy-1)) {
    it->x += 1;
    if (it->x == grid->stepsx) {
      it->x=0; it->y += 1;
    }
    it->vid=MATRIX(grid->startidx, it->x, it->y);
  }
}

igraph_integer_t igraph_2dgrid_next(igraph_2dgrid_t *grid, 
			      igraph_2dgrid_iterator_t *it) {
  long int ret=it->vid;

  if (ret==0) { return 0; }

  /* First neighbor */
  it->ncells=-1;
  if (it->x != grid->stepsx-1) {
    it->ncells += 1;
    it->nx[it->ncells]=it->x+1;
    it->ny[it->ncells]=it->y;
  }
  if (it->y != grid->stepsy-1) {
    it->ncells += 1;
    it->nx[it->ncells]=it->x;
    it->ny[it->ncells]=it->y+1;
  }
  if (it->ncells==1) {
    it->ncells += 1;
    it->nx[it->ncells]=it->x+1;
    it->ny[it->ncells]=it->y+1;
  }
  it->ncells+=1;
  it->nx[it->ncells]=it->x;
  it->ny[it->ncells]=it->y;
  
  it->nei=VECTOR(grid->next) [ ret-1 ];
  while (it->ncells > 0 && it->nei==0 ) {
    it->ncells -= 1;
    it->nei=MATRIX(grid->startidx, it->nx[it->ncells], it->ny[it->ncells]);
  }

  /* Next vertex */
  it->vid=VECTOR(grid->next)[ it->vid-1 ];
  while ( (it->x < grid->stepsx-1 || it->y<grid->stepsy-1) &&
	  it->vid == 0) {
    it->x += 1;
    if (it->x == grid->stepsx) {
      it->x=0; it->y += 1;
    }
    it->vid=MATRIX(grid->startidx, it->x, it->y);
  }

  return ret;
}

inline igraph_integer_t igraph_2dgrid_next_nei(igraph_2dgrid_t *grid,
				 igraph_2dgrid_iterator_t *it) {
  long int ret=it->nei;

  if (it->nei != 0) {
    it->nei=VECTOR(grid->next) [ ret-1 ];
  }
  while (it->ncells > 0 && it->nei==0 ) {
    it->ncells -= 1;
    it->nei=MATRIX(grid->startidx, it->nx[it->ncells], it->ny[it->ncells]);
  }
 
  return ret;  
}

/*-----------------------------------------------------------------------*/

inline int igraph_i_layout_mergegrid_which(igraph_i_layout_mergegrid_t *grid,
					   igraph_real_t xc, igraph_real_t yc,
					   long int *x, long int *y) {
  if (xc <= grid->minx) { 
    *x=0; 
  } else if (xc >= grid->maxx) {
    *x=grid->stepsx-1;
  } else {
    *x=floor((xc-(grid->minx))/(grid->deltax));
  }

  if (yc <= grid->miny) {
    *y=0;
  } else if (yc >= grid->maxy) {
    *y=grid->stepsy-1;
  } else {
    *y=floor((yc-(grid->miny))/(grid->deltay));
  }

  return 0;
}  

int igraph_i_layout_mergegrid_init(igraph_i_layout_mergegrid_t *grid,
				   igraph_real_t minx, igraph_real_t maxx, long int stepsx,
				   igraph_real_t miny, igraph_real_t maxy, long int stepsy) {
  grid->minx=minx;
  grid->maxx=maxx;
  grid->stepsx=stepsx;
  grid->deltax=(maxx-minx)/stepsx;
  grid->miny=miny;
  grid->maxy=maxy;
  grid->stepsy=stepsy;
  grid->deltay=(maxy-miny)/stepsy;
  
  grid->data=Calloc(stepsx*stepsy, long int);
  if (grid->data==0) {
    IGRAPH_ERROR("Cannot create grid", IGRAPH_ENOMEM);
  }
  return 0;
}

void igraph_i_layout_mergegrid_destroy(igraph_i_layout_mergegrid_t *grid) {
  Free(grid->data);
}

#define MAT(i,j) (grid->data[(grid->stepsy)*(j)+(i)])
#define DIST2(x2,y2) (sqrt(pow(x-(x2),2)+pow(y-(y2), 2)))

int igraph_i_layout_merge_place_sphere(igraph_i_layout_mergegrid_t *grid,
				       igraph_real_t x, igraph_real_t y, igraph_real_t r, 
				       long int id) {
  long int cx, cy;
  long int i, j;

  igraph_i_layout_mergegrid_which(grid, x, y, &cx, &cy);
  
  MAT(cx, cy)=id+1;

#define DIST(i,j) (DIST2(grid->minx+(cx+(i))*grid->deltax, \
			 grid->miny+(cy+(j))*grid->deltay))
  
  for (i=0; cx+i<grid->stepsx && DIST(i,0)<r; i++) {
    for (j=0; cy+j<grid->stepsy && DIST(i,j)<r; j++) {
      MAT(cx+i,cy+j)=id+1;
    }
  }

#undef DIST
#define DIST(i,j) (DIST2(grid->minx+(cx+(i))*grid->deltax, \
                         grid->miny+(cy-(j)+1)*grid->deltay))

  for (i=0; cx+i<grid->stepsx && DIST(i,0)<r; i++) {
    for (j=1; cy-j>0 && DIST(i,j)<r; j++) {
      MAT(cx+i,cy-j)=id+1;
    }
  }

#undef DIST
#define DIST(i,j) (DIST2(grid->minx+(cx-(i)+1)*grid->deltax, \
			 grid->miny+(cy+(j))*grid->deltay))

  for (i=1; cx-i>0 && DIST(i,0)<r; i++) {
    for (j=0; cy+j<grid->stepsy && DIST(i,j)<r; j++) {
      MAT(cx-i,cy+j)=id+1;
    }
  }

#undef DIST
#define DIST(i,j) (DIST2(grid->minx+(cx-(i)+1)*grid->deltax, \
			 grid->miny+(cy-(j)+1)*grid->deltay))

  for (i=1; cx-i>0 && DIST(i,0)<r; i++) {
    for (j=1; cy-j>0 && DIST(i,j)<r; j++) {
      MAT(cx-i,cy-j)=id+1;
    }
  }  

#undef DIST
#undef DIST2

  return 0;
}

long int igraph_i_layout_mergegrid_get(igraph_i_layout_mergegrid_t *grid,
				       igraph_real_t x, igraph_real_t y) {
  long int cx, cy;
  long int res;

  if (x <= grid->minx || x >= grid->maxx ||
      y <= grid->miny || y >= grid->maxy) {
    res=-1;
  } else {
    igraph_i_layout_mergegrid_which(grid, x, y, &cx, &cy);
    res=MAT(cx, cy)-1;
  }

  return res;
}

#define DIST2(x2,y2) (sqrt(pow(x-(x2),2)+pow(y-(y2), 2)))

long int igraph_i_layout_mergegrid_get_sphere(igraph_i_layout_mergegrid_t *grid,
					      igraph_real_t x, igraph_real_t y, igraph_real_t r) {
  long int cx, cy;
  long int i,j;
  long int ret;
  
  if (x-r <= grid->minx || x+r >= grid->maxx ||
      y-r <= grid->miny || y+r >= grid->maxy) {
    ret=-1;
  } else {
    igraph_i_layout_mergegrid_which(grid, x, y, &cx, &cy);
    
    ret=MAT(cx, cy)-1;
  
#define DIST(i,j) (DIST2(grid->minx+(cx+(i))*grid->deltax, \
			 grid->miny+(cy+(j))*grid->deltay))
  
    for (i=0; ret<0 && cx+i<grid->stepsx && DIST(i,0)<r; i++) {
      for (j=0; ret<0 && cy+j<grid->stepsy && DIST(i,j)<r; j++) {
	ret=MAT(cx+i,cy+j)-1;
      }
    }
    
#undef DIST
#define DIST(i,j) (DIST2(grid->minx+(cx+(i))*grid->deltax, \
                         grid->miny+(cy-(j)+1)*grid->deltay))
    
    for (i=0; ret<0 && cx+i<grid->stepsx && DIST(i,0)<r; i++) {
      for (j=1; ret<0 && cy-j>0 && DIST(i,j)<r; j++) {
	ret=MAT(cx+i,cy-j)-1;
      }
    }
    
#undef DIST
#define DIST(i,j) (DIST2(grid->minx+(cx-(i)+1)*grid->deltax, \
			 grid->miny+(cy+(j))*grid->deltay))
    
    for (i=1; ret<0 && cx-i>0 && DIST(i,0)<r; i++) {
      for (j=0; ret<0 && cy+j<grid->stepsy && DIST(i,j)<r; j++) {
	ret=MAT(cx-i,cy+j)-1;
      }
    }
    
#undef DIST
#define DIST(i,j) (DIST2(grid->minx+(cx-(i)+1)*grid->deltax, \
			 grid->miny+(cy-(j)+1)*grid->deltay))
    
    for (i=1; ret<0 && cx+i>0 && DIST(i,0)<r; i++) {
      for (j=1; ret<0 && cy+i>0 && DIST(i,j)<r; j++) {
	ret=MAT(cx-i,cy-j)-1;
      }
    }  
    
#undef DIST
    
  }
  
  return ret;
}
  
/* int print_grid(igraph_i_layout_mergegrid_t *grid) { */
/*   long int i,j; */
  
/*   for (i=0; i<grid->stepsx; i++) { */
/*     for (j=0; j<grid->stepsy; j++) { */
/*       printf("%li ", MAT(i,j)-1); */
/*     } */
/*     printf("\n"); */
/*   } */
/* } */