/* r_soundgl.c in hallway with walls, perfect reflection                   */
/*            simulate sound wave propegation                              */
/*            db = 10 log_10(Vout/Vin)    Vout<Vin, db negative            */
/*            10^(db/10) = Vout/Vin, choose Vin=1.0, Vout=1.0 at 0 db      */
/*            Vout = 10^(db/10) called  "V" V^2 is power                   */
/* drawing has 0 degrees up for antenna, x=0, y=1, use +90 degrees in code */
/* change 'delta' for faster or slower display                             */
/* Ultrasonic Ranger SRF08 used for antenna beam pattern, -60db detect     */
/* 'curve' is instantaneous radius of curvature of conical bean wavefront  */
/* the signal drops off as the square of distance traveled                 */
/* thus, the signal drops off as the fourth power of the range             */
/* (this is ignoring friction losses and interference patterns             */
/* The wavelength at 40KHz is 8.65mm or  0.00865cm                         */

#include <GL/glut.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#undef min
#undef max
#undef abs
#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define abs(a)   ((a)<0.0?(-(a)):(a))
#undef M_PI
#define M_PI  3.14159265358979323846

static double minx = -300.0; /* graphics sizes */
static double maxx =  300.0;
static double miny = -600.0;
static double maxy =  300.0;
static double minz =   -1.0;
static double maxz =    1.0;
static int width   = 400;
static int height  = 600; /* added beam pattern */
static double next = 0.0;
static double scale = 10.0;
static int maxmoves = 1600; /* a way to stop simulation, a function of delta */
static int moves    = 0;

/* speed of sound = 345.0  meters per second */
/*                = 34,500 centimeters per second */
static double sound = 34.5;  /* centimeters per millisecond */
static double delta = 0.01;   /* step size, milliseconds=3.45cm */
static double pulse = 0.1; /* 0.1 millisectond, 3.45 cm */
static double t = 0.0;       /* time in milliseconds */
static int np = 0;           /* number of sound packets */
struct packet {double x; double y; double dx; double dy;
               double xold; double yold;
               double len; double curve; double v;};
               /* a sound packet 3.45 cm long, arc center at x,y
                  direction of travel dx,dy, arc length len,
                  curvature(radius)[distance traveled] curve, strength v
                  xold and yold used for intersection */
static struct packet p[10000]; /* each individual packet moving in two dimensions */
/* beam pattern, 5 degree increments for first 13, values in db */
static double ant_db[22] = {  0.0,   0.0,  -1.0,  -2.0,  -3.0,  -5.0,  -7.0, /* db  */
                            -10.0, -14.0, -17.0, -19.0, -21.0, -23.0, -26.0,
                            -30.0, -24.0, -31.0, -28.0, -29.0, -36.0, -28.0, -34.0};
static double ant_ang[22]= {  0.0,   5.0,  10.0,  15.0,  20.0,  25.0,  30.0, /* deg */
                             35.0,  40.0,  45.0,  50.0,  55.0,  60.0,  70.0,
                             75.0,  90.0, 110.0, 120.0, 130.0, 150.0, 170.0, 180.0};
static int num_ant_db = 22;

static double curve0; /* radius change in delta time, 3.45*delta */
static int nwalls = 4; /* number of wall segments */
typedef double wall[4]; /* x1,y1,  x2,y2 */
static wall walls[4]={{-22.4, -2.0, -22.4, 20.0},
                      { 22.4, -2.0,  22.4, 20.0},
                      {-22.4, 20.0,  22.4, 20.0},
                      { -1.7,  0.0,   1.7,  0.0}}; /* antenna */
static double wall_min = -2.0;

typedef GLfloat col[3];  /* r, g, b */
static col col_db[10] = {{0.0, 0.0, 0.0},  /* black  */
                         {1.0, 0.0, 0.0},  /* red    */
                         {1.0, 1.0, 0.0},  /* orange */
                         {0.0, 1.0, 1.0},  /* yellow */
                         {0.0, 1.0, 0.0},  /* green  */
                         {0.0, 0.0, 1.0},  /* blue   */
                         {1.0, 0.0, 1.0},  /* purple */
                         {0.4, 0.4, 0.4},  /* dk gray*/
                         {0.6, 0.6, 0.6},  /* gray   */
                         {0.9, 0.9, 0.9}}; /* lt gray*/

static void display(void);
static void init();
static void mouse(int button, int state, int x, int y);
static void reshape(int w, int h);
static void simulation_init();
static void simulation_step(void);
static int intersect(double x1, double y1, double x2, double y2,
                     double x3, double y3, double x4, double y4,
                     double *xii, double *yii);
static void reflect(double dx, double dy,
                    double x3, double y3, double x4, double y4,
                    double *dxr, double *dyr);
static void draw_beam_pat(double xc, double yc, double zc);
static void draw_circle(double xc, double yc, double zc, double r, int segments);
static void draw_text(double x, double y, double z, double scale, char * msg);
static void draw_col_db();

static void simulation_init()
{
  int i, j, k, kk, nph;
  double x1, y1, x2, y2;

  double ant_vr[13];
  double dx0[13];
  double dy0[13];
  double v0 = 1.0;
  double ang = 0.0;
  double dang = 5.0; /* initial 5 deg steps */
  double len0;

  printf("r_soundgl \n");

  printf("ang           X      Y        DX     DY     L       R \n");
  curve0 = 3.45 * delta;
  len0 = 2.0*M_PI*curve0*dang/360.0; /* arc length 2 Pi r / 72 */
  np = 0;
  p[np].x = 0.0;
  p[np].y = curve0;
  p[np].xold = 0.0;
  p[np].yold = curve0;
  p[np].dx = 0.0;
  p[np].dy = 1.0;
  p[np].len = len0;
  p[np].curve = curve0;
  p[np].v = v0;
  printf("%4.0f p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
         ang, np, p[np].x, p[np].y, p[np].dx, p[np].dy, 
         p[np].len, p[np].curve, p[np].v);
  np++;
  ang = ang + dang;

  for(i=1; i<13; i++) /* initial antenna radiation */
  {
    ant_vr[i] = pow(10.0, ant_db[i]/10.0);
    dx0[i] = sin(M_PI*ang/180.0);
    dy0[i] = cos(M_PI*ang/180.0); 
    p[np].xold = p[np].x;
    p[np].yold = p[np].y;
    p[np].x = curve0*dx0[i];
    p[np].y = curve0*dy0[i];
    p[np].dx = dx0[i];
    p[np].dy = dy0[i];
    p[np].len = len0;
    p[np].curve = curve0;
    p[np].v = v0*ant_vr[i];
    printf("%4.0f p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
           ang, np, p[np].x, p[np].y, p[np].dx, p[np].dy, 
           p[np].len, p[np].curve, p[np].v);
    np++;
    p[np].xold = p[np].x;
    p[np].yold = p[np].y;
    p[np].x = -curve0*dx0[i];
    p[np].y = curve0*dy0[i];
    p[np].dx = -dx0[i];
    p[np].dy = dy0[i];
    p[np].len = len0;
    p[np].curve = curve0;
    p[np].v = v0*ant_vr[i];
    printf("%4.0f p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
           -ang, np, p[np].x, p[np].y, p[np].dx, p[np].dy, 
           p[np].len, p[np].curve, p[np].v);
    np++;
    ang = ang + dang;
  }
  t = t + delta;
  glutPostRedisplay();
} /* end simulation_init */

static void simulation_step(void)
{
  int j, k, kk, nph;
  double x1, y1, x2, y2;
  double split = 1.0; /* 1.0cm length for splitting packet */
  int first_split = 0;
  double hang, thang; /* move angle (dx=cos,dy=sin) on split */
  double dyn, dys; /* sin(x+/-y)=sin(x)cos(y)+/-cos(x)sin(y) */
  double dxn, dxs; /* cos(x+/-y)=cos(x)cos(y)-/+sin(x)sin(y) */
  int hit;
  double xii, yii, len1;
  double dxr, dyr;
  double rang, gain_db, gain_v, vrec, dbrec, v_dist, db_dist;
         /* received angle, gain, V and db */
  int iang;

  moves = moves + 1;
  /* if(moves > maxmoves) exit(0); */
  if(np==0) exit(0); /* no more waves */
  
  first_split = 0;
  nph = np;
  for(j=0; j<nph; j++) /* do not split new ones */
  {
    x1 = p[j].x+p[j].len*p[j].dy;
    y1 = p[j].y-p[j].len*p[j].dx;
    x2 = p[j].x-p[j].len*p[j].dy;
    y2 = p[j].y+p[j].len*p[j].dx;
    p[j].xold = p[j].x;
    p[j].yold = p[j].y;
    p[j].x = p[j].x + curve0*p[j].dx; /* move packet */
    p[j].y = p[j].y + curve0*p[j].dy;
    p[j].len = p[j].len *(p[j].curve+curve0)/p[j].curve;
    p[j].curve = p[j].curve + curve0;
    if(j==9999)
    {
        printf("p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
               j, p[j].x, p[j].y, p[j].dx, p[j].dy, 
               p[j].len, p[j].curve, p[j].v);
    }
    if(p[j].len>split) /* split into 2, 1/2 len */
    {
      if(first_split==0)
      {
        printf("p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
               j, p[j].x, p[j].y, p[j].dx, p[j].dy, 
               p[j].len, p[j].curve, p[j].v);
      }
      thang = p[j].len*180.0/(p[j].curve*M_PI); /* degrees */
      hang = 0.25*p[j].len/p[j].curve; /* 1/4 in radians */
      dxs = cos(hang);
      dys = sin(hang);
      dxn = p[j].dx*dxs-p[j].dy*dys;
      dyn = p[j].dy*dxs+p[j].dx*dys;
      if(first_split==0)
      {
        printf("hang=%g, dxs=%g, dys=%g \n thang=%g, dx=%g, dy=%g, dxn=%g, dyn=%g \n",
               hang, dxs, dys, thang, p[j].dx, p[j].dy, dxn, dyn);
      }
      p[np].xold = p[j].xold +0.25*p[j].len*p[j].dy; /* approximation */
      p[np].yold = p[j].yold -0.25*p[j].len*p[j].dx;
      p[np].x = p[j].x +0.25*p[j].len*p[j].dy;
      p[np].y = p[j].y -0.25*p[j].len*p[j].dx;
      p[np].dx = p[j].dx*dxs+p[j].dy*dys;
      p[np].dy = p[j].dy*dxs-p[j].dx*dys; 
      p[np].len = p[j].len/2.0;
      p[np].curve = p[j].curve;
      p[np].v = p[j].v;
      p[j].xold = p[j].xold -0.25*p[j].len*p[j].dy;
      p[j].yold = p[j].yold +0.25*p[j].len*p[j].dx;
      p[j].x = p[j].x -0.25*p[j].len*p[j].dy;
      p[j].y = p[j].y +0.25*p[j].len*p[j].dx;
      p[j].dx = dxn;
      p[j].dy = dyn;
      p[j].len = p[j].len/2.0;
      /* p[j].curve stays same */
      /* p[j].v stays same */
      if(first_split==0)
      {
        printf("p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
               j, p[j].x, p[j].y, p[j].dx, p[j].dy, 
               p[j].len, p[j].curve, p[j].v);
        printf("p[%d]={%6.2f,%6.2f  %6.2f,%6.2f  %6.2f, %6.2f, V=%g \n",
               np, p[np].x, p[np].y, p[np].dx, p[np].dy, 
               p[np].len, p[np].curve, p[np].v);
        first_split = 1;
      }
      np++;
    }
  }
  /* printf("time %7.4f, np=%d \n", t, np);                                 */
  /* printf("%4.0f p[%d]={%6.3f,%6.3f  %6.3f,%6.3f  %6.3f, %6.3f, V=%g \n", */
  /*        0.0, 0, p[0].x, p[0].y, p[0].dx, p[0].dy,                       */
  /*        p[0].len, p[0].curve, p[0].v);                                  */

  /* detect packet that hit wall, perfect reflection in this run */
  nph = np; /* in case packet split on wall */
  for(j=0; j<nph; j++) /* no new packets created, no energy loss */
  {
    /* test if packet intersected wall */
    for(kk=0; kk<(nwalls*2-1); kk++) /* may get double bounce in corner */
    {
      k = kk%3; /* built in 3 walls checks again against all three */
      hit = intersect(p[j].x, p[j].y, p[j].xold, p[j].yold,
          walls[k][0], walls[k][1], walls[k][2], walls[k][3],
                      &xii, &yii);
      if(hit)
      {
        len1 = sqrt((xii-p[j].x)*(xii-p[j].x)+
                    (yii-p[j].y)*(yii-p[j].y));
        /* printf("hit=%d at p[%d] %6.2f, %6.2f, %g \n", hit, j, xii, yii, len1); */
        /* printf("x1=%6.2f y1=%6.2f x2=%6.2f y2=%6.2f \n",                       */
        /*        p[j].x, p[j].y, p[j].xold, p[j].yold);                          */
        /* printf("x3=%6.2f y3=%6.2f x4=%6.2f y4=%6.2f \n",                       */
        /*        walls[k][0], walls[k][1], walls[k][2], walls[k][3]);            */
        /* printf("hit xy=%6.2f,%6.2f  old=%6.2f,%6.2f  d=%6.2f,%6.2f, V=%g \n",  */
        /*         p[j].x, p[j].y, p[j].xold, p[j].yold, p[j].dx, p[j].dy,        */
        /*         p[j].v);                                                       */

        reflect(p[j].dx, p[j].dy,
                walls[k][0], walls[k][1], walls[k][2], walls[k][3],
                &dxr, &dyr);
        p[j].dx = dxr; /* general case */
        p[j].dy = dyr;
        p[j].x = xii + len1*p[j].dx; /* should move away from wall */
        p[j].y = yii + len1*p[j].dy;
        p[j].xold = xii+0.01*dxr; /* prevent immediate wall intersection */
        p[j].yold = yii+0.01*dyr;
      }
    }
  }
  /* delete sound pulses outside boundary */
  if(t<sound*pulse+0.001) goto nodetect;
  for(j=0; j<np; j++) /* delete sound pulses outside area of interest */
  {
    if(p[j].y<wall_min)
    {
      p[j] = p[np-1];
      np = np - 1;
      j = j - 1;   /* could be  p[j--]=p[--np]; */
    }
    else if(p[j].x<-25.0 || p[j].x>25.0 || p[j].y>21.0)
    {
      printf("out xy=%6.2f,%6.2f  old=%6.2f,%6.2f  d=%6.2f,%6.2f, V=%g %d \n",
             p[j].x, p[j].y, p[j].xold, p[j].yold, p[j].dx, p[j].dy, 
             p[j].v, k);
      p[j] = p[np-1];
      np = np - 1;
      j = j - 1;   /* could be  p[j--]=p[--np]; */
    }
  }

  /* detect reciever hits */
  vrec = 0.0;
  for(j=0; j<np; j++) /* detect reciever hits */
  {
    /* assumes antenna center at 0,0 */
    if(p[j].x>-1.7 && p[j].x<1.7 && p[j].y>0.0 && p[j].y<3.54*delta)
    {
      /* a detection, get angle to interpolate antenna gain */
      rang = abs(atan2(p[j].dx,p[j].dy))*180.0/M_PI; /* degrees off 0 as up */
      if(rang>90.0) rang = abs(rang-180.0);
      iang = rang/5.0; /* lower bound, use as index */
      if(iang<13-1) /* must be in +/- 60degree range */
      {
        gain_db = ant_db[iang] + ((rang-(double)iang*5.0)/5.0)*
                                  (ant_db[iang+1]-ant_db[iang]);
        gain_v = pow(10.0, gain_db/10.0); 
        printf("detection at %g milliseconds, angle=%g degrees, gain_db=%g, gain_v=%g \n",
               t, rang, gain_db, gain_v);
        v_dist = p[j].v/(p[j].curve*p[j].curve);
        db_dist = 10.0*log10(v_dist);
        printf("            db_dist=%g, v_dist=%g, db_tot=%g, v_tot=%g\n",
                db_dist, v_dist, db_dist+gain_db, v_dist*gain_v);
        
        vrec = vrec + v_dist * gain_v;
      }
    }
  }
  if(vrec != 0.0)
  {
    dbrec = 10.0 * log10(vrec);
    printf("detected vrec=%g, dbrec=%g \n", vrec, dbrec);
  }                
  /* finished this time step of simulation of sound propegation */
nodetect:
  glutPostRedisplay();
  t = t + delta;
} /* end simulation_step */


/* return 0 for no intersect, 1 for intersect at xi, yi */
/* (yi-y1)*(x2-x1) = (xi-x1)*(y2-y1) */
/* (yi-y3)*(x4-x3) = (xi-x3)*(y4-y3) two eqn in 2 unknown */
/* yi*x21 - y1*x21 = xi*y21 - x1*y21 or yi*x21 - xi*y21 = y1*x21 - x1*y21 */
/* yi*x43 - y3*x43 = xi*y43 - x3*y43 or yi*x43 - xi*y43 = y3*x43 - x3*y43 */
/* yi = (y1*x21 - x1*y21 + xi*y21)/x21 */
/* xi =-(y1*x21 - x1*y21 - yi*x21)/y21 */
/* yi = (y3*x43 - x3*y43 + xi*y43)/x43 */
/* xi =-(y3*x43 - x3*y43 - yi*x43)/y43 */

static int intersect(double x1, double y1, double x2, double y2,
                     double x3, double y3, double x4, double y4,
                     double *xii, double *yii)
{
  double x21, y21, x43, y43, xd, xn;
  double xi, yi;
  double eps = 0.00001;
  double smax = 99.9;
  
  x21 = x2-x1;
  y21 = y2-y1;
  x43 = x4-x3;
  y43 = y4-y3;
  /* handle special cases, most common and faster */
  if(abs(x21)<eps) /* verticle wall */
  {
    if(abs(x43)<eps) goto fail;
    xi = x1;
    yi = (y3*x43-x3*y43+xi*y43)/x43;
  }
  else if(abs(y21)<eps) /* horizontal wall */
  {
    if(abs(y43)<eps) goto fail;
    yi = y1;
    xi = -(y3*x43-x3*y43-yi*x43)/y43;
  }
  else if(abs(x43)<eps) /* verticle beam */
  {
    xi = x3;
    yi = (y1*x21-x1*y21+xi*y21)/x21;
  }
  else if(abs(y43)<eps) /* horizontal beam */
  {
    yi = y3;
    xi = -(y1*x21-x1*y21-yi*x21)/y21;
  }
  else /* general case, no division problems */
  {
    xd = x43*y21 - x21*y43;
    xn = x21*(y3*x43-x3*y43) - x43*(y1*x21-x1*y21);
    xi = xn/xd;
    yi = (y1*x21-x1*y21+xi*y21)/x21;
  }

  /* check that intersection within finite lines */
  /* x1,y1 x2,y2  and  x3,y3 x4,y4 */
  if((xi<min(x1,x2)-eps) || (xi>max(x1,x2)+eps)) goto fail;
  if((yi<min(y1,y2)-eps) || (yi>max(y1,y2)+eps)) goto fail;
  if((xi<min(x3,x4)-eps) || (xi>max(x3,x4)+eps)) goto fail;
  if((yi<min(y3,y4)-eps) || (yi>max(y3,y4)+eps)) goto fail;
  *xii = xi;
  *yii = yi;
  return 1;
fail:
  *xii = smax;
  *yii = smax;
  return 0;
}

static void reflect(double dx, double dy,
                    double x3, double y3, double x4, double y4,
                    double *dxr, double *dyr)
{
  double dx1, dy1, dx2, dy2, dxt, dyt, dxp, dyp, dx3, dy3, d;

  dx2 = x4 - x3;
  dy2 = y4 - y3;

  dxt = dx/sqrt(dx*dx+dy*dy); /* normalize */
  dyt = dy/sqrt(dx*dx+dy*dy);
  dx1 = dxt;
  dy1 = dyt;
  dxt = dx2/sqrt(dx2*dx2+dy2*dy2); /* normalize */
  dyt = dy2/sqrt(dx2*dx2+dy2*dy2);
  dx2 = dxt;
  dy2 = dyt;
  dxp = dy2; /* flip to get perpendicular, either sign possible */
  dyp = -dx2; /* flip to get perpendicular */

  dxt = dxp*dy1-dyp*dx1; /* theta_perpendicular-theta_beam */
  dyt = dyp*dy1+dxp*dx1;

  dx3 = dxp*dyt+dyp*dxt;
  dy3 = dyp*dyt-dxp*dxt;

  *dxr = -dx3;
  *dyr = -dy3;
}

static void display(void)
{
  int i, j, dbi;
  static int first = -24;
  double range, dbf;
  
  /* clear window */
  glClear(GL_COLOR_BUFFER_BIT); 
  glLoadIdentity ();

  /* Draw antenna beam pattern */
  draw_beam_pat(0.0, -400.0, 0.0);
  
  /* Draw db color scale */
  draw_col_db();
  
  /* Draw the sound waves */
  glColor3f(1.0, 0.0, 0.0);
  glPointSize(4.0);
  for(i=0; i<np; i++)
  {
    range = 0.25*p[i].curve;
    if(range<1.0) range=1.0;
    dbf = 10.0*log10(p[i].v/(range*range));
    dbi = (int)(-dbf);
    dbi = dbi/6;
    if(first<0)
    {
      first++;
      printf("i=%d, dbi=%d, dbf=%g, p[i].v=%g, range=%g \n",
              i, dbi, dbf, p[i].v, range);
    }
    if(dbi>9) dbi=9;
    if(dbi<0) dbi=0;
    glColor3fv(col_db[dbi]);
    glBegin(GL_POINTS);
      glVertex3d(scale*p[i].x, scale*p[i].y, 0.0);
    glEnd();
  }

  /* Draw the walls */
  glColor3f(0.0, 0.0, 1.0);
  for(j=0; j<nwalls; j++)
  {
    glBegin(GL_LINES);
      glVertex3d(scale*walls[j][0], scale*walls[j][1], 0.0);
      glVertex3d(scale*walls[j][2], scale*walls[j][3], 0.0);
    glEnd();
  }

  /* draw text, in its own context */
  glColor3f(0.0, 0.0, 0.0);
  draw_text(-150.0,  -50.0, 0.0, 0.2, "sound wave propegation");
  draw_text(-150.0, -590.0, 0.0, 0.2, "antenna beam pattern");
  
  glFlush(); 
  glutSwapBuffers();

}

static void draw_text(double x, double y, double z, double scale, char * msg)
{
  char * tp;
  
  glPushMatrix();
    glLoadIdentity ();
    glEnable(GL_LINE_SMOOTH);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glEnable(GL_BLEND);
    glTranslatef(x, y, z);
    glScalef(scale, scale, 0.0);
    for(tp=msg; *tp; tp++)
      glutStrokeCharacter(GLUT_STROKE_ROMAN, *tp);
  glPopMatrix();
}

static void draw_col_db()
{
  int i;
  GLfloat x = -280.0;
  
  glPointSize(10.0);
  for(i=0; i<9; i++)
  {
    glColor3fv(col_db[i]);
    glBegin(GL_POINTS);
      glVertex3d(x, -80.0, 0.0);
      glVertex3d(x+10.0, -80.0, 0.0);
      glVertex3d(x+20.0, -80.0, 0.0);
    glEnd();
    x = x + 66.0;
  }
  glColor3f(0.0, 0.0, 0.0);
  draw_text(-290.0, -110.0, 0.0, 0.08,
  "0 -6    -6 -12   -12 -18  -18 -24  -24 -30  -30 -36  -36 -42  -42 -48  -48 -54");
  draw_text(-100.0, -130.0, 0.0, 0.1, "signal strength in db");
}

/* This routine handels mouse events */
static void mouse(int button, int state, int x, int y)
{
  float wx, wy;
  
  if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN)
  {
    /* Translate back to our coordinate system */
    wx = ((maxx-minx) * x) / (float)(width - 1) - 1.0;
    wy = ((maxx-minx) * (height - 1 - y)) / (float)(height - 1) - 1.0;
    printf("x=%d, y=%d, wx=%g, wy=%g \n", x, y, wx, wy);

  }
  if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN)
  {
    /* available */
  }
  glutPostRedisplay();
}

/* This routine handles window resizes */
static void reshape(int w, int h)
{
  width = w;
  height = h;
  /* Set the transformations */
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(minx, maxx, miny, maxy, minz, maxz);
  glMatrixMode(GL_MODELVIEW);
  glViewport(0, 0, w, h);
}

static void init()
{
  /* set clear color to white */
  glClearColor (1.0, 1.0, 1.0, 0.0);
  /* set fill  color to black */
  glColor3f(0.0, 0.0, 0.0);

  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  glOrtho(minx, maxx, miny, maxy, minz, maxz);
  glMatrixMode (GL_MODELVIEW);
  glViewport(0, 0, width, height);
}

static void draw_beam_pat(double xc, double yc, double zc)
{
  double s60, c60;
  double r1, r2, x1, x2, y1, y2;
  int i;
  
  s60 = sin(M_PI/3.0);
  c60 = 0.5;
  
  glColor3f(0.0, 0.0, 0.0);
  /* draw constant db circles */
  draw_circle(xc, yc, zc,  20.0, 32); /* -36db */
  draw_circle(xc, yc, zc,  40.0, 32); /* -30db */
  draw_circle(xc, yc, zc,  60.0, 64); /* -24db */
  draw_circle(xc, yc, zc,  80.0, 64); /* -18db */
  draw_circle(xc, yc, zc, 100.0, 64); /* -12db */
  draw_circle(xc, yc, zc, 120.0, 64); /*  -6db */
  draw_circle(xc, yc, zc, 140.0, 64); /*   0db r=140+db*20.0/6.0 */
  /* label db circles */
  draw_text(  4.0+xc, 128.0+yc, zc, 0.1, "0");
  draw_text(  2.0+xc, 108.0+yc, zc, 0.1, "-6");
  draw_text(  2.0+xc,  88.0+yc, zc, 0.1, "-12");
  draw_text(  2.0+xc,  68.0+yc, zc, 0.1, "-18");
  draw_text(  2.0+xc,  48.0+yc, zc, 0.1, "-24");
  draw_text(  2.0+xc,  28.0+yc, zc, 0.1, "-30");
  draw_text(  2.0+xc,   8.0+yc, zc, 0.1, "-36");
  
  /* draw angle lines every 30 degrees */
  glBegin(GL_LINES);
    glVertex3d(-140.0+xc,            yc, zc);
    glVertex3d( 140.0+xc,            yc, zc);
    glVertex3d(       xc,     -140.0+yc, zc);
    glVertex3d(       xc,      140.0+yc, zc);
    glVertex3d(-140.0*s60+xc, -140.0*c60+yc, zc);
    glVertex3d( 140.0*s60+xc,  140.0*c60+yc, zc);
    glVertex3d(-140.0*c60+xc, -140.0*s60+yc, zc);
    glVertex3d( 140.0*c60+xc,  140.0*s60+yc, zc);
    glVertex3d( 140.0*s60+xc, -140.0*c60+yc, zc);
    glVertex3d(-140.0*s60+xc,  140.0*c60+yc, zc);
    glVertex3d( 140.0*c60+xc, -140.0*s60+yc, zc);
    glVertex3d(-140.0*c60+xc,  140.0*s60+yc, zc);
  glEnd();
  /* label angle lines */
  draw_text(      -5.0+xc,           150.0+yc,      zc, 0.1, "0");
  draw_text( 150.0*c60+xc,         150*s60+yc,      zc, 0.1, "30");
  draw_text( 150.0*s60+xc,         150*c60+yc,      zc, 0.1, "60");
  draw_text(     150.0+xc,            -5.0+yc,      zc, 0.1, "90");
  draw_text( 150.0*c60+xc,      -150.0*s60+yc-10.0, zc, 0.1, "120");
  draw_text( 150.0*s60+xc,      -150.0*c60+yc,      zc, 0.1, "150");
  draw_text(     -10.0+xc,          -160.0+yc,      zc, 0.1, "180");
  draw_text(-150.0*c60+xc-20.0, -150.0*s60+yc-10.0, zc, 0.1, "210");
  draw_text(-150.0*s60+xc-20.0, -150.0*c60+yc-10.0, zc, 0.1, "240");
  draw_text(    -150.0+xc-20.0,       -5.0+yc,      zc, 0.1, "270");
  draw_text(-150.0*s60+xc-20.0,  150.0*c60+yc,      zc, 0.1, "300");
  draw_text(-150.0*c60+xc-20.0,  150.0*s60+yc,      zc, 0.1, "330");
  /* draw beam pattern */
  for(i=0; i<num_ant_db-1; i++)
  {
    r1 = 140.0+ant_db[i]*20.0/6.0;
    r2 = 140.0+ant_db[i+1]*20.0/6.0;
    x1 = r1*cos((ant_ang[i]+90.0)*M_PI/180.0);
    y1 = r1*sin((ant_ang[i]+90.0)*M_PI/180.0);
    x2 = r2*cos((ant_ang[i+1]+90.0)*M_PI/180.0);
    y2 = r2*sin((ant_ang[i+1]+90.0)*M_PI/180.0);
    glBegin(GL_LINES);
      glVertex3d(x1+xc, y1+yc, zc);
      glVertex3d(x2+xc, y2+yc, zc);
      glVertex3d(-x1+xc, y1+yc, zc);
      glVertex3d(-x2+xc, y2+yc, zc);
    glEnd();
  }
}

static void draw_circle(double xc, double yc, double zc, double r, int segments)
{
  /* set line color and thickness before call */
  int i, seg;
  double ang, dang, x1, y1, x2, y2;
  
  seg = ((segments+3)/4)*4; /* must be a multiple of 4 */
  if(seg < 8) seg = 8;
  dang = (M_PI * 2.0)/(double)seg;
  ang = 0.0;
  x1 = r*cos(ang)+xc;
  y1 = r*sin(ang)+yc;
  for(i=0; i<seg; i++)
  {
    ang = ang+dang;
    x2 = r*cos(ang)+xc;
    y2 = r*sin(ang)+yc;
    glBegin(GL_LINES);
      glVertex3d(x1, y1, zc);
      glVertex3d(x2, y2, zc);
    glEnd();
    x1 = x2;
    y1 = y2;
  }
}

int main(int argc, char* argv[])
{
  glutInit(&argc,argv);
  glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);  
  glutInitWindowSize(width, height);
  glutInitWindowPosition(100,100); 
  
  glutCreateWindow(argv[0]); 
  glutDisplayFunc(display);
  glutMouseFunc(mouse);
  glutIdleFunc(simulation_step);
  init();
  simulation_init();
  glutMainLoop();
  return 0;
}