#include "Swarming.H"

/***************************************************************************
 * This subroutine simply plot the positions of all the current particles
 * while their directions are drawn as a small line section.
 ***************************************************************************/
void Plot_Swarming(int ifirst, int ilast, int ori_last, double *x, double *y,
		   double *u, double *v)
{
  /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Find min,max data values (for the upper and lower bound of axes).
c ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  double xlo=x[0]-0.5;
  double xhi=x[0]+0.5;
  double xlolo=xlo;
  double xhihi=xhi;
  for (int ic=ifirst;ic<=ilast;ic++) {
    double xi=x[ic];
    if (xi<xlo) xlo=xi;
    else if(xi<xlolo) xlolo=xi;
    if (xi>xhi) xhi=xi;
    else if(xi>xhihi) xhihi=xi;
    }
  // xlo=xlolo;           // use the second limit as the figure boundary
  // xhi=xhihi; 
  double ylo=y[0]-0.5;
  double yhi=y[0]+0.5;
  double ylolo=ylo;
  double yhihi=yhi;
  for (int ic=ifirst;ic<=ilast;ic++) {
    double yi=y[ic];
    if (yi<ylo) ylo=yi;
    else if(yi<ylolo) ylolo=yi;
    if (yi>yhi) yhi=yi;
    else if(yi>yhihi) yhihi=yi;
  }
  //  ylo=ylolo;          // use the second limit as the figure boundary
  //  yhi=yhihi;

  /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Calculate the horizontal and vertical lengths as well as the origin.
c ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  double centerx, centery, hlength, vlength;
  hlength = xhi-xlo;
  vlength = yhi-ylo;
  centerx = (xhi+xlo)*0.5;
  centery = (yhi+ylo)*0.5;
  /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c      In order to keep the figure as a square, we replace the shorter 
c   length with the longer one and recalculate the upper and lower bound
c   of the corresponding axis.
c      We also extend the boundary by 0.5 unit so that we will have a 
c   better view of the particles exactly on the edges.
c ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(hlength > vlength) {
    yhi = centery + 0.5*hlength +0.5;
    ylo = centery - 0.5*hlength -0.5;
    xhi = xhi + 0.5;
    xlo = xlo - 0.5;
    hlength = hlength +1.0;
    vlength = hlength;
  }
  else {
    xhi = centerx + 0.5*vlength +0.5;
    xlo = centerx - 0.5*vlength -0.5;
    yhi = yhi + 0.5;
    ylo = ylo - 0.5;
    vlength = vlength +1.0;
    hlength = vlength;
  }
  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Calculate the relative location of the origin in the figure.
c   (because the figure is setup as a (0,1)x(0,1) box.)
c +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  centerx = (0.0-xlo)/hlength;
  centery = (0.0-ylo)/vlength;

  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   The OpenGL drawing routine begins.
c +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  glClear(GL_COLOR_BUFFER_BIT);

  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Draw the axes in black.
c +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  glColor3f(0.0, 0.0, 0.0);  
  gtDrawAxis(xlo, xhi, ylo, yhi);

  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Draw the original particles in blue.
c   (ic = ifirst to ori_last)
c +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  glBegin(GL_LINES); 

  int ic=ifirst;
  double width=0.01;
  double ptx = x[ic]/hlength + centerx;
  double pty = y[ic]/vlength + centery;
  double ptu = u[ic]/hlength;
  double ptv = v[ic]/vlength;
  double vlen = sqrt(ptu*ptu+ptv*ptv);
  glColor3f(0.0, 0.0, 1.0);
  glVertex2f(ptx-width, pty);
  glVertex2f(ptx+width, pty);
  glVertex2f(ptx, pty-width);
  glVertex2f(ptx, pty+width);
  if(vlen!=0.) {
    glVertex2f(ptx, pty);
    glVertex2f(ptx+0.05*ptu/vlen, pty+0.05*ptv/vlen);
  }
  for (ic=ifirst+1;ic<=ori_last;ic++) {
    double ptx = x[ic]/hlength + centerx;
    double pty = y[ic]/vlength + centery;
    double ptu = u[ic]/hlength;
    double ptv = v[ic]/vlength;
    double vlen = sqrt(ptu*ptu+ptv*ptv);
    glVertex2f(ptx-width, pty);
    glVertex2f(ptx+width, pty);
    glVertex2f(ptx, pty-width);
    glVertex2f(ptx, pty+width);
    if(vlen!=0) {
      glVertex2f(ptx, pty);
      glVertex2f(ptx+0.05*ptu/vlen, pty+0.05*ptv/vlen);
    }
  }
  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Draw the added particles in red.
c   (ic = ori_last+1 to ilast)
c +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  for (ic=ori_last+1;ic<=ilast;ic++) {
    double ptx = x[ic]/hlength + centerx;
    double pty = y[ic]/vlength + centery;
    double ptu = u[ic]/hlength;
    double ptv = v[ic]/vlength;
    double vlen = sqrt(ptu*ptu+ptv*ptv);
    glColor3f(1.0, 0.0, 0.0);
    glVertex2f(ptx-width, pty);
    glVertex2f(ptx+width, pty);
    glVertex2f(ptx, pty-width);
    glVertex2f(ptx, pty+width);
    if(vlen!=0) {
      glVertex2f(ptx, pty);
      glVertex2f(ptx+0.05*ptu/vlen, pty+0.05*ptv/vlen);
    }
  }

  glEnd();
}

/************************************************************************
 *    This subroutine draw a rectangle diagram, whose x-axis is defined *
 * between 0 and max_dist with rhoGrid mesh grids. The values at each   *
 * grid site are stored in the density array.                           *
 ************************************************************************/
void Plot_RadialDensity(int rhoGrid, double max_dist, double *density)
{
  double max_den=0.;

  /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Look for maximum density (for the upper bound of y-axis)
c ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  for(int ie=0; ie<rhoGrid; ie++) {
    if(density[ie]>max_den)
      max_den = density[ie];
  }

  glClear(GL_COLOR_BUFFER_BIT);

  /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Set color to black and draw the axes.
c ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  glColor3f(0.0, 0.0, 0.0);
  gtDrawAxis(0., max_dist, 0., max_den);

  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
c   Set color to blue and draw the rectangles.
c +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
  glColor3f(0.0, 0.0, 1.0);
  for(int ie=0; ie<rhoGrid; ie++) {
    double dx = 1./rhoGrid;
    glRectd((double)(ie+0.05)*dx, 0., 
	    (double)(ie+0.95)*dx, density[ie]/max_den);
  }

  glEnd();
}