#include "Swarming.H"

#define rhoGrid 300          // Number of mesh grids while computing density

struct linearad_common {
  double statelft,statergt,alpha,beta,ca,la,cr,lr,lc;
};
extern linearad_common F77NAME(linearad);     //model parameters

struct machine_common {
  double roundoff,small,huge,undefind;        //machine dependent constants
};
extern machine_common F77NAME(machine);

int ncells;         // number of particles
int gcells;         // number of "ghost particles"
int nsteps;         // total number of time steps (not used anymore)
int stpstp;         // output results every 'stpstp' time steps
double tmax;        // total simulation time (not used anymore)
double tstp;        // time step length
int nonlin_f;       // whether to use non-linear propelsion (not used anymore)

int fm;             // first mesh grid label (not used here)
int lm;             // last mesh grid label (not used here)
int ifirst;         // first partical label 
int ilast;          // last partical label
int ori_last;       // original last partical label (not including ghosts)

double *xold;       // old position and velocity
double *xnew;       // new position and velocity
double *xm;         // position array used by RK4 method
double *vm;         // velocity array used by RK 4 method
double *km, *km1, *km2, *km3, *Gforce;   // used by numerical methods
double *x, *y, *u, *v, *temp;     // defining x, y, u, v arrays
double rho[6*rhoGrid];            // array to store radial density...etc.

double polar,momen;              // polarization and angular momentum

// The following are variables used by the OpenGL library.
int swarming_id, plot_id;                          // window id's
int halt_n=0;                                      // halt the simulation?
int add_p=0;                                       // adding particles?
int finish_add_p=1;                                // finish adding?
double add_x, add_y, add_u, add_v;                 // the pos/vel of the added
int reset_time=0;                                  

int main(int argc,char* argv[]) {
  void simulation_default(void);
  void simulation_init(void);
  void main_display(void);
  void plot_display(void);
  void graph_init(void);
  void idle(void);
  void keyCB(unsigned char, int, int);
  void mouseCB(int, int, int, int);
  void motionCB(int, int);
  void entryCB(int);
  void sw_menu(int);

   /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     1. set the default constants and parameter values       
     2. read parameters from the input file                  
     3. initiate the variable arrays with initial conditions 
   +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

    simulation_default();
    readinput(argc, argv, &ncells, &gcells, &nsteps, &stpstp, 
	      &tmax, &tstp, &nonlin_f);
    simulation_init();

   /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    initiate the graphics
   +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    glutInitWindowPosition(-1,-1);
    glutInitWindowSize(size_W,size_H);
    swarming_id = glutCreateWindow("Swarming");

   /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    Create a menu
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

    glutCreateMenu(sw_menu);
    glutAddMenuEntry("Continue", 1);
    glutAddMenuEntry("Pause", 2);
    glutAddMenuEntry("Pause and save data as sw.dat", 3);
    glutAddMenuEntry("Increase velocity by 10%%", 4);
    glutAddMenuEntry("Quit", 5);
    glutAttachMenu(GLUT_RIGHT_BUTTON);

   /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    initiate graph and define call back subroutines
   +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

    graph_init();
    glutDisplayFunc(main_display);
    glutIdleFunc(idle);
    glutKeyboardFunc(keyCB);
    glutMouseFunc(mouseCB);
    glutMotionFunc(motionCB);
    glutEntryFunc(entryCB);

   /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    Create a second window 
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
    glutInitWindowSize(300,300);
    plot_id = glutCreateWindow("Momentum and Polarization");

   /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    initiate the second window and define its call back subroutines
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

    graph_init();
    glutDisplayFunc(plot_display);

    glutMainLoop();
    return(0);
}

/********************************************************************
 *   This subroutine sets default values to the machine-dependent   *
 * constants and the model parameters.                              *
 ********************************************************************/

void simulation_default(void)
{
//  set machine-dependent constants for Fortran
    F77NAME(machine).roundoff=DBL_EPSILON;
    F77NAME(machine).small=DBL_MIN;
    F77NAME(machine).huge=DBL_MAX;
    F77NAME(machine).undefind=HUGE_VAL;

//  set default values for problem parameters
    F77NAME(linearad).statelft=500.;
    F77NAME(linearad).statergt=-500.;
    F77NAME(linearad).alpha=10.;
    F77NAME(linearad).beta=1.;
    F77NAME(linearad).ca=0.4;
    F77NAME(linearad).la=40.;
    F77NAME(linearad).cr=1.;
    F77NAME(linearad).lr=20.;
    F77NAME(linearad).lc=5.;
    ncells=10;
    gcells=10;
    nsteps=10;
    stpstp=10;
    tmax=1.;
    tstp=0.01;
    nonlin_f=0;
}

/*****************************************************************
 *   This subroutine creates arrays to store particle positions  *
 * and velocities as well as arrays needed by the RK4 method.    *
 *    It then sets the initial conditions for the simulation.    *
 *****************************************************************/
void simulation_init(void)
{
//  array bounds for Fortran calls
    fm=0;       
    lm=ncells + gcells -1;
    ifirst=0;
    ilast=ncells-1;
    ori_last=ilast;
    ncells = ncells + gcells;

//  since ncells is determined dynamically, 
//  must use dynamic memory allocation

    xold=(double*)malloc(4*ncells*sizeof(double));
    xnew=(double*)malloc(4*ncells*sizeof(double));
    xm=(double*)malloc(2*ncells*sizeof(double));
    vm=(double*)malloc(2*ncells*sizeof(double));
    km=(double*)malloc(4*ncells*sizeof(double));
    km1=(double*)malloc(4*ncells*sizeof(double));
    km2=(double*)malloc(4*ncells*sizeof(double));
    km3=(double*)malloc(4*ncells*sizeof(double));
    Gforce=(double*)malloc(2*ncells*sizeof(double));
    x=xold;
    y=xold+ncells;
    u=xold+2*ncells;
    v=xold+3*ncells;

//  The variables are initiated with a huge value first
//  and then re-initiated to the desired initial conditions
    {
      for (int i=0;i<4*ncells;i++) {
        xold[i]=HUGE_VAL;
        xnew[i]=HUGE_VAL;
        km[i]=HUGE_VAL;
      }
      for (int i=0;i<2*ncells;i++) {
        xm[i]=HUGE_VAL;
        vm[i]=HUGE_VAL;
      }  
      reinitiate(ncells, ifirst,ilast,xold);
    }
}

/*****************************************************************
 *    This subroutine sets up the graphic default properties     *
 *****************************************************************/
void graph_init(void)
{
    // define background and foreground default colors
    glClearColor(1.0,1.0,1.0,1.0);
    glColor3f(0.0, 0.0, 0.0);

    // define graphic projection matrix
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluOrtho2D(0, 1, 0, 1);
}

/*****************************************************************
 *    The display subroutine of the main simulation window.      *
 *****************************************************************/
void main_display(void)
{
  Plot_Swarming(ifirst,ilast,ori_last,x,y,u,v);
  glutSwapBuffers();
}

/******************************************************************
 *    This subrotine is supposed to be the display subroutine of  *
 * the second window, but it's empty now.                         *
 ******************************************************************/
void plot_display(void)
{
  glClear(GL_COLOR_BUFFER_BIT);
  glutSwapBuffers();
}

/*******************************************************************
 *    The idle subroutine defines what the program should do while *
 * there is no input events from either the keyboard or the mouse. *
 *******************************************************************/
void idle(void)
{
  void running(void);

   /*+++++++++++++++++++++++++++++++++++++++
   If halt_n=0, run the simulation; 
   if halt_n=1, do not run the simulation;
   and when the simulation is not running,
   see if there are particle-adding events
   +++++++++++++++++++++++++++++++++++++++++*/
  if(halt_n==0)
    running();
  else if(add_p == 1)
    AddParticle();
}

/******************************************************************
 *    The keyboard event subroutine, defining the functions for   *
 * keyboard input events.                                         *
 ******************************************************************/
void keyCB(unsigned char key, int mx, int my)
{
  if(key == 'r' || key == 'R')
    halt_n=0;
  else if(key == 's' || key == 'S')
    halt_n=1;
  else if(key == 'q' || key == 'Q') {
    delete [] xold;
    delete [] xnew;
    delete [] xm;
    delete [] vm;
    delete [] km;
    exit(0);
  }
}

/**********************************************************************
 *   The mouse event subroutine, defining the functions for mouse     *
 * clicking.                                                          *
 **********************************************************************/
void mouseCB(int button, int state, int mx, int my)
{
  if(state == GLUT_DOWN && button == GLUT_LEFT_BUTTON && halt_n ==1) {
    add_p = 1;
    finish_add_p = 0;
    add_x = (double)mx/(double)size_W;
    add_y = 1.0-(double)my/(double)size_H;
    add_u = 0.;
    add_v = 0.;
    printf("add_x=%lf, add_y=%lf\n", add_x, add_y);
  }
  if(state == GLUT_UP && button == GLUT_LEFT_BUTTON && add_p ==1) {
    finish_add_p = 1;
  }
}

/***********************************************************************
 *  The mouse motion subroutine, defining the function for mouse       *
 * motion.                                                             *
 ***********************************************************************/
void motionCB(int mx, int my)
{
  if(halt_n == 1) {
    add_u = (double)mx/(double)size_W;
    add_v = 1.0-(double)my/(double)size_H;
  }
}

/***********************************************************************
 *   The mouse entry subroutine, defineing the function for the        *
 * situation when mouse leaves the window.                             *
 ***********************************************************************/
void entryCB(int mstate)
{
  if(mstate == GLUT_LEFT) add_p = 0;
}

/***********************************************************************
 *   The menu call back subroutine, defining the function for each     *
 * menu items.                                                         *
 ***********************************************************************/
void sw_menu(int menu_value)
{
  if(menu_value == 1) halt_n = 0;
  if(menu_value == 2) halt_n = 1;
  if(menu_value == 3) {
    char filename[10];
    sprintf(filename, "sw.dat");
    writeout(ncells, ifirst, ilast, xold, filename);
    halt_n = 1;
  }
  if(menu_value == 4) {
    for(int ic=ifirst; ic<=ilast; ic++) {
      xold[2*ncells+ic] *= 1.1;
      xold[3*ncells+ic] *= 1.1;
      xnew[2*ncells+ic] *= 1.1;
      xnew[3*ncells+ic] *= 1.1;
    }
    reset_time = 1;
  }
  if(menu_value == 5) exit(0);
}

/*******************************************************************
 * Running the swarming simulation.                                *
 *******************************************************************/
void running(void)
{
  static double t=0.;
  static double print_t=30.;
  static double dt = tstp;
  static long step;

  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    some time step resetting situations:
    1. if step=100000, reset to 0 (so that it won't exceed the limit
    2. if adding new particles is finished, reset step to 0, so that
       the simulation will do the first 4 steps using the RK4 method.
    3. if reset_time=1, reset time and time step.
    +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
 
 if(step==100000) step=0;
  if(finish_add_p == 2) {
    step=0;
    finish_add_p = 1;
  }
  if(reset_time==1) {
    step=0;
    t=0.;
    reset_time=0;
  }

  /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    Simulation: first 5 steps are using the 4th order Runge-Kutta method;
                then, the 4-step Adam-Bashforth method are used.
  +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

  if(step<5)
    F77NAME(method)(t,dt,fm,lm,ifirst,ilast, xold, xnew, xm, vm, km);
  else
    F77NAME(fourstep)(t,dt,fm,lm,ifirst,ilast,xold,xnew,km,km1,km2,km3,
		      Gforce);
  t+=dt;
  step++;
  {
    temp = xold;
    xold = xnew;
    xnew = temp;

    temp = km3;
    km3 = km2;
    km2 = km1;
    km1 = km;
    km = temp;

    x = xold;
    y = xold+ncells;
    u = xold+2*ncells;
    v = xold+3*ncells;
  }

  /*++++++++++++++++++++++++++++++++++++++++++++++++++++
    Draw the current configuration every (stpstp) steps
  ++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

  if((step%stpstp)==0 && t>0.) {
    glutSetWindow(swarming_id);
    glutPostRedisplay();
  }
  /*
  if(t>100.) {
    void counting_dist(void);
    counting_dist();
  }
  */

  if((step%100)==0) {
    /*
    double ek=0.; 
    double ek2=0.;
    double ek_now;
    double n;

    n=(double)(ilast-ifirst+1);
    for(int ic=ifirst;ic<=ilast;ic++) {
      ek_now = 0.5*(u[ic]*u[ic]+v[ic]*v[ic]);
      ek += ek_now;
      ek2 += ek_now*ek_now;
    }
    ek = ek/n;
    ek2 = sqrt(ek2/n);
    //    printf("Ek= %lf,  Sqrt(Ek^2)= %lf, diff= %lf  diff\%= %lf\n", ek, ek2, fabs(ek-ek2), fabs(ek-ek2)/ek*100.);
    printf("%lf  %lf  %lf  %lf  %lf\n", t, ek, ek2, fabs(ek-ek2), fabs(ek-ek2)/ek*100.);
    */

    {
      static double dr, max_dist;
      static int avg_count;
      int cw,ccw;
      double max_den;
      double *density;
      static double acc_density[rhoGrid], acc_momr[rhoGrid],acc_momt[rhoGrid],
	acc_sigk[rhoGrid], acc_sigv[rhoGrid];

      glutSetWindow(plot_id);
      glClear(GL_COLOR_BUFFER_BIT);

      F77NAME(group)(fm,lm,ifirst,ilast,xnew,polar,momen,rhoGrid,rho,dr,
		     cw,ccw,Gforce);

      if((momen<0.995 && momen>-0.995)&&(t<200.)) {
	max_dist = dr*rhoGrid;
	dr = 0.; // reset dr
	for(int ie=0; ie<rhoGrid; ie++) {
	  acc_density[ie]=0.;
	  acc_momr[ie]=0.;
	  acc_momt[ie]=0.;
	  acc_sigk[ie]=0.;
	  acc_sigv[ie]=0.;
	}
	avg_count=0;
	density = rho+rhoGrid;
      }
      else {
	for(int ie=0; ie<rhoGrid; ie++) {
	  acc_density[ie] = (acc_density[ie]*avg_count+rho[rhoGrid+ie])
	                   / (avg_count+1);
	  acc_momr[ie] = (acc_momr[ie]*avg_count+rho[2*rhoGrid+ie])
	                   / (avg_count+1);
	  acc_momt[ie] = (acc_momt[ie]*avg_count+rho[3*rhoGrid+ie])
	                   / (avg_count+1);
	  acc_sigk[ie] = (acc_sigk[ie]*avg_count+rho[4*rhoGrid+ie])
	                   / (avg_count+1);
	  acc_sigv[ie] = (acc_sigv[ie]*avg_count+rho[5*rhoGrid+ie])
	                   / (avg_count+1);
	    }
	printf("acc%d  %lf  %lf  %lf  %lf  %lf  %lf\n", avg_count,t, acc_density[50], acc_momr[50], acc_momt[50], acc_sigk[50], acc_sigv[50]);
	avg_count++;
	density = acc_density;
      }
      //      printf("dr=%lf, max_dist=%lf, avg_count=%d\n", dr, max_dist, avg_count);
      
      max_den = 0.;
      for(int ie=0; ie<rhoGrid; ie++) {
	if(density[ie]>max_den)
	  max_den = density[ie];
      }

      glColor3f(0.0, 0.0, 0.0);
      gtDrawAxis(0., max_dist, 0., max_den);
      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);
      }
      if(avg_count==200) {
	FILE *fpw;
	fpw = fopen("den_t.dat", "w");
	for (int ie=0;ie<rhoGrid; ie++) {
          double radi=dr*((double)ie+0.5);
          double area=2*3.1415926535*radi*dr;
	  if(density[ie]<0.0001)
	    fprintf(fpw, "%lf  %lf  %lf  %lf  %lf  %lf\n", radi, 
		    density[ie]/area, 0., 0., 0., 0.);
	  else
	    fprintf(fpw, "%lf  %lf  %lf  %lf  %lf  %lf\n", radi, 
		    density[ie]/area, acc_momr[ie]/density[ie], 
		    acc_momt[ie]/density[ie], acc_sigk[ie],acc_sigv[ie]);
	}
	fclose(fpw);
	exit(0);
      }

      //      printf("t=%lf, polar=%lf, momen=%lf\n", t, polar, momen);
      printf("%2.0f  %lf  %lf  %d  %d\n", t, polar, momen, cw, ccw);
      glEnd();
      glutSwapBuffers();
    }
  }
}

/*************************************************************************
 * This subroutine adds particles by reading mouse click and motion.
 *   The first half of the subroutine is the same as Plot_Swarming in
 * the file plotSwarming.C while the second half adds the new particle 
 * and plot it into the graph.
 ************************************************************************/
void AddParticle(void)
{
  static int add_count=0;
    
//      find min,max data values
  double xlo=x[0];
  double xhi=x[0];
  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];
  double yhi=y[0];
  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 = (0.0-xlo)/hlength;
  centery = (0.0-ylo)/vlength;
    
  gtDrawAxis(xlo, xhi, ylo, yhi);

//    draw numerical solution
  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);
  //  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);
    //    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);
    //    glColor3f(0.0, 1.0, 0.0);
    glVertex2f(ptx, pty);
    glVertex2f(ptx+0.05*ptu/vlen, pty+0.05*ptv/vlen);
  }

  if(add_count<gcells) {
    double ptu = add_u - add_x;
    double ptv = add_v - add_y;
    double vlen = sqrt(ptu*ptu+ptv*ptv);
    if(vlen!=0) {
      ptu = ptu/vlen;
      ptv = ptv/vlen;
    }
    glColor3f(1.0, 0.0, 0.0);
    glVertex2f(add_x-width, add_y);
    glVertex2f(add_x+width, add_y);
    glVertex2f(add_x, add_y-width);
    glVertex2f(add_x, add_y+width);
    //    glColor3f(0.0, 1.0, 0.0);
    glVertex2f(add_x, add_y);
    glVertex2f(add_x+0.05*ptu, add_y+0.05*ptv);

    if(finish_add_p == 1) {
      add_count++;
      ilast++;
      int ic=ilast;
      double width=0.01;
      double speed;
      xold[ic] = (add_x - centerx)*hlength;
      xnew[ic] = xold[ic];
      xold[ic+ncells] = (add_y - centery)*vlength;
      xnew[ic+ncells] = xold[ic+ncells];
      if(F77NAME(linearad).beta!=0.)
	speed = sqrt(F77NAME(linearad).alpha/F77NAME(linearad).beta);
      else
	speed = 1.0;
      xold[ic+2*ncells] = ptu*speed;
      xnew[ic+2*ncells] = xold[ic+2*ncells];
      xold[ic+3*ncells] = ptv*speed;
      xnew[ic+3*ncells] = xold[ic+3*ncells];
      printf("real_x=%lf, real_y=%lf, real_u=%lf, real_v=%lf\n", 
	     xold[ic], xold[ic+ncells], xold[ic+2*ncells], xold[ic+3*ncells]);

      add_p = 0;
      finish_add_p = 2;
    }
  }
  else {
    printf("You've used up all the ghost cells!\n");
    add_p = 0;
    finish_add_p = 1;
  }

  glEnd();

  glColor3f(0.0, 0.0, 0.0);
  glutSwapBuffers();
}

void counting_dist(void)
{
  double xdist, ydist, dist;
  double min_dist;
  FILE *fpa;

  fpa = fopen("cntdist.dat", "a");

  for(int ic=ifirst; ic<=ilast; ic++) {
    min_dist = 10000.;
    for(int jc=ifirst;jc<=ilast; jc++) {
      if (ic!=jc) {
	xdist = (x[ic]-x[jc]);
	ydist = (y[ic]-y[jc]);
	dist = sqrt(xdist*xdist+ydist*ydist);
	if (dist<min_dist)
	  min_dist = dist;
      }
    }
    fprintf(fpa, "%lf   \n", min_dist);
  }
  
  fclose(fpa);
}