#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)
{
//      find min,max data values
  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;
  //  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;
  //  yhi=yhihi;
  
  glClear(GL_COLOR_BUFFER_BIT);
  glColor3f(0.0, 0.0, 0.0);
  
  double centerx, centery, hlength, vlength;
  hlength = xhi-xlo;
  vlength = yhi-ylo;
  centerx = (xhi+xlo)*0.5;
  centery = (yhi+ylo)*0.5;
  
  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;
  }
  centerx = (0.0-xlo)/hlength;
  centery = (0.0-ylo)/vlength;
  

  gtDrawAxis(xlo, xhi, ylo, yhi);
  //  printf("xlo=%lf, xhi=%lf, ylo=%lf, yhi=%lf\n", xlo, xhi, ylo, yhi);

//    draw numerical solution
  glBegin(GL_LINES);
//    gluOrtho2D(-3, 3, 0, 2);

  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.) {
      //      glColor3f(1.0, 0.0, 0.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);
    //      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) {
      //	glColor3f(1.0, 0.0, 0.0);
      glVertex2f(ptx, pty);
      glVertex2f(ptx+0.05*ptu/vlen, pty+0.05*ptv/vlen);
    }
  }
  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) {
      //	glColor3f(0.0, 1.0, 0.0);
      glVertex2f(ptx, pty);
      glVertex2f(ptx+0.05*ptu/vlen, pty+0.05*ptv/vlen);
    }
  }
  glEnd();
}