/* See stmap.h */
/* Last edited on 2004-08-18 00:17:48 by stolfi */

#include <stmap.h>
#include <stmatrix.h>
#include <stheap.h>

#include <ps.h>
#include <js.h>
#include <r2.h>
#include <fget.h>
#include <nget.h>

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <values.h>

/* INTERNAL PROTOTYPES */

void st_map_file_error(int nlin, char *msg);
void st_map_add_edge(Map *m, int org, int dst, int ei, float c0, float c1);
bool st_map_insert_edge_in_ring(quad_arc e, quad_arc r);
int  st_map_ccw(Point p, Point q, Point r);
bool st_map_vertex_is_visible(Point p, Interval xr, Interval yr);
bool st_map_edge_is_visible(Point p, Point q, Interval xr, Interval yr);

/* IMPLEMENTATIONS */

Map *st_map_read(FILE *f)
  { Map *m = (Map *)malloc(sizeof(Map));
    int vi;
    int ei, ai;
    int nlin = 0;
    int res;

    affirm(m != NULL, "out of memory"); 
    res = fscanf(f, "p street-graph %d %d\n", &(m->nv), &(m->ne));
    nlin++;
    fprintf(stderr, "res = %d\n", res);
    if (res != 2) { st_map_file_error(nlin, "bad header line"); }
    m->out = (quad_arc *)malloc(m->nv * sizeof(quad_arc));
    m->along = (quad_arc *)malloc(2 * m->ne * sizeof(quad_arc));
    m->vd = (VertexData **)malloc(m->nv * sizeof(VertexData *));
    m->ed = (EdgeData **)malloc(m->ne * sizeof(EdgeData *));
    
    for (vi = 0; vi < m->nv; vi++) { m->vd[vi] = NULL; m->out[vi] = NULL_REF; }
    for (ei = 0; ei < m->ne; ei++) { m->ed[ei] = NULL; }
    for (ai = 0; ai < 2*m->ne; ai++) { m->along[ai] = NULL_REF; }

    for (vi = 0; vi < m->nv; vi++) 
      { VertexData *vd = (VertexData *)malloc(sizeof(VertexData));
        int vot, vin, id;
        res = fscanf(f, "v %d %lf %lf %d %d\n", &id, &(vd->p.c[0]), &(vd->p.c[1]), &vin, &vot);
        nlin++;
        if (res != 5) { st_map_file_error(nlin, "bad vertex line"); }
        affirm(id == vi, "vertices out of order");
        vd->id = id; vd->deg = vin+vot;
        m->vd[id] = vd;
      }
    for (ei = 0; ei < m->ne; ei++)
      { int org, dst, blocked;
        float c0, c1;
        res = fscanf(f, "a %d %d %d\n", &org, &dst, &blocked);
        nlin++;
        if (res != 3) { st_map_file_error(nlin, "bad edge line"); }
        if (blocked == 1) 
          { c0 = c1 = MAXFLOAT; }
        else
          { double d = r2_dist(&(m->vd[org]->p), &(m->vd[dst]->p));
            c0 = c1 = (float)d;
          }
        st_map_add_edge(m, org, dst, ei, c0, c1);
      }
    return m;
  }
  
#define DIRID(a) ((((EdgeData *)LDATA(a))->id << 1) | SYMBIT(a))

void st_map_init_costs(Map *m, float *d, quad_arc *e, float *c)
  { int vi, ai;
    for(vi = 0; vi < m->nv; vi++) { d[vi] = MAXFLOAT; e[vi] = NULL_REF; }
    for(ai = 0; ai < 2*m->ne; ai++) { c[ai] = MAXFLOAT; }
  }

void st_map_reset_costs(Map *m, int *r, int nr, float *d, quad_arc *e, float *c)
  { int k;
    for (k = 0; k < nr; k++)
      { int vi = r[k]; 
        d[vi] = MAXFLOAT; e[vi] = NULL_REF;
        quad_arc b = m->out[vi], a = b;
        do
          { EdgeData *ad = (EdgeData *)LDATA(a);
            int s = SYMBIT(a);
            int ai = 2 * ad->id + s;
            c[ai] = MAXFLOAT;
            a = ONEXT(a);
          }
        while (a != b);
      }
  }

void st_map_compute_costs
  ( Map *m, 
    int u, 
    float dMax,
    int *r,
    int *nr, 
    float *d, 
    quad_arc *e,
    float *c
  )
  {
    int vi;
    st_Heap *h = st_heap_new(2*m->ne);
    float cprev = 0.0, dprev = 0.0;

    /* Let DIRID(a) be the ID number of the *directed* edge {a},
      namely {DIRID(a) = 2*ei+s} where {ei} is the ID of the
      undirected edge, and {s = SYMBIT(a)}. The value of {c[DIRID(a)]}
      is the cost of the optimal path from {u} that ends with the
      quad_arc {a}; or is {MAXFLOAT} if that cost is still unknown. */
    
    affirm(dMax >= 0.0, "invalid {dMax}");

    (*nr) = 0;
    d[u] = 0.0;
    vi = u; 
    do
      { /* The next reached vertex in order of increasing cost is {vi}. */
        /* Consistency check: */
        affirm(d[vi] >= dprev, "vertices popped out of order");
        dprev = d[vi];
        /* Store {vi} in list of reached vertices: */
        r[*nr] = vi; (*nr)++;
        /* Insert in heap all fresh edges out of {vi}, truncating at {dMax}: */
        { quad_arc b = m->out[vi], a = b;
          do
            { EdgeData *ad = (EdgeData *)LDATA(a);
              int s = SYMBIT(a);
              int ai = 2 * ad->id + s;
              float cnew = d[vi] + ad->cost[s];
              affirm(cnew >= d[vi], "negative arc cost");
              if (cnew <= dMax)
                { VertexData *vd = (VertexData *)DDATA(a);
                  int wi = vd->id;
                  affirm(c[ai] == MAXFLOAT, "total arc cost c[ai] set twice");
                  c[ai] = cnew; 
                  if (cnew < d[wi]) { st_heap_insert(h, ai, c); }
                }
              a = ONEXT(a);
            }
          while (a != b);
        }
        /* Get next vertex {vi} in cost order, or -1 if none: */
        vi = -1;
        while ((h->n > 0) && (vi < 0))
          { int ai = st_heap_pop(h, c);
            quad_arc a = m->along[ai];
            VertexData *vd = (VertexData *)DDATA(a);
            int wi = vd->id;
            affirm(c[ai] >= cprev, "total arc costs popped out of order");
            cprev = c[ai];
            if (c[ai] < d[wi]) { d[wi] = c[ai]; e[wi] = a; vi = wi; }
          }
      }
    while (vi >= 0);
    st_heap_discard(h);
  }
     
void st_compute_coverage
  ( Map *m, 
    int *u, 
    float *dMax, 
    int n, 
    int *vcover,
    int *ecover
  )
  {
    int vi, ei;
    for (vi = 0; vi < m->nv; vi++) { vcover[vi] = 0; }
    for (ei = 0; ei < m->ne; ei++) { ecover[ei] = 0; }
    if (n > 0) 
      { /* Work files for {st_map_compute_costs}: */
        float d[m->nv];
        float c[2 * m->ne];
        quad_arc e[m->nv];
        int r[m->nv];
        int nr;
        /* Count sites that cover each vertex and each edge: */
        st_map_init_costs(m, d, e, c);
        int i;
        for(i = 0; i < n; i++)
          { int ui = u[i];
            float dmi = dMax[i];
            /* Find vertices within cost {dMax} from {ui}: */
            st_map_compute_costs(m, ui, dmi, r, &nr, d, e, c);
            /* Tally vertex and edge coverage: */
            st_increment_coverage(m, dmi, r, nr, d, c, vcover, ecover);
            /* Reset costs for next {st_map_compute_costs}: */
            st_map_reset_costs(m, r, nr, d, e, c);
          }
      }
  }
  
void st_increment_coverage
  ( Map* m,
    float dMax, 
    int *r, 
    int nr, 
    float *d, 
    float *c,
    int *vcover, 
    int *ecover
  )
  { int k;
    for (k = 0; k < nr; k++)
      { int vi = r[k];
        /* Tally vertex coverage: */
        affirm(d[vi] <= dMax, "inconsistent cost");
        vcover[vi]++;
        /* Tally edge coverage: */
        quad_arc b = m->out[vi], a = b;
        do
          { EdgeData *ad = (EdgeData *)LDATA(a);
            int ei = ad->id;
            int ai = 2 * ei, aj = ai + 1;
            /* Check if edge {ei} is {dMax}-reachable: */
            if ((c[ai] <= dMax) || (c[aj] <= dMax))
              { /* Take care not to increment {ecover[ei]} twice: */
                VertexData *wd = (VertexData *)ODATA(SYM(a));
                int wi = wd->id;
                if (vi < wi) { ecover[ei]++; }
              }
            a = ONEXT(a);
          }
        while (a != b);
      }
  }

void st_map_plot(
    FILE *f, 
    Map *m, 
    Interval xr, Interval yr, 
    float *vwidth, RGBColor *vcolor,
    float *ewidth, RGBColor *ecolor
  )
  {
    int vi, ei;
    bool clipping = FALSE;
    
    if ( 
        ( (xr.lo <= xr.hi) && (yr.lo <= yr.hi) ) &&
        ( (xr.lo != -MAXDOUBLE) || (xr.hi != MAXDOUBLE) ||
          (yr.lo != -MAXDOUBLE) || (yr.hi != MAXDOUBLE)
        )
      )
      { clipping = TRUE; }
    else
      { clipping = FALSE; }

    /* Plot edges: */
    for (ei = 0; ei < m->ne; ei++)
      { quad_arc a0 = m->along[2*ei];
        if (a0 != NULL_REF)
          { VertexData *od = (VertexData *)ODATA(a0);
            VertexData *dd = (VertexData *)DDATA(a0);
            if ((! clipping) || st_map_edge_is_visible(od->p, dd->p, xr, yr))
              { float pr,pg,pb,pwd;
                if (ecolor != NULL) 
                  { RGBColor *cp = &(ecolor[ei]); pr = cp->R; pg = cp->G; pb = cp->B; }
                else
                  { pr = 1.0; pg = 0.0; pb = 0.0; }
                pwd = (ewidth != NULL ? ewidth[ei] : 0.25);
                if (pwd > 0.0)
                  { ps_set_pen(f, pr,pg,pb, pwd, 0.0, 0.0);
                    ps_draw_segment(f, od->p.c[0], od->p.c[1], dd->p.c[0], dd->p.c[1]);
                  }
              }
          }
      }
      
    /* Plot vertices: */
    for (vi = 0; vi < m->nv; vi++)
      { VertexData *vd = m->vd[vi];
        if ((! clipping) || st_map_vertex_is_visible(vd->p, xr, yr))
          { float pr,pg,pb,pwd;
            if (vcolor != NULL) 
              { RGBColor *cp = &(vcolor[vi]); pr = cp->R; pg = cp->G; pb = cp->B; }
            else
              { pr = 1.0; pg = 0.0; pb = 0.0; }
            pwd = (vwidth != NULL ? vwidth[vi] : (vd->deg == 2 ? 0.00 : 0.50));
            /* Plot dot: */
            if (pwd > 0.0)
              { ps_set_pen(f, pr,pg,pb, pwd, 0.0, 0.0);
                ps_draw_segment(f, vd->p.c[0], vd->p.c[1], vd->p.c[0], vd->p.c[1]);
              }
          }
      }
  }
  
void st_map_get_bbox(Map *m, Interval *xr, Interval *yr)
  {
    double xlo = 0.0, xhi = 0.0, ylo = 0.0, yhi = 0.0;
    int i;
    for (i = 0; i < m->nv; i++)
      { VertexData *vd = m->vd[i];
        double x = vd->p.c[0], y = vd->p.c[1];
        if (i == 0)
          { xlo = xhi = x; ylo = yhi = y; }
        else
          { if (x < xlo) { xlo = x; } else if (x > xhi) { xhi = x; }
            if (y < ylo) { ylo = y; } else if (y > yhi) { yhi = y; }
          }
      }
    xr->lo = xlo; xr->hi = xhi;
    yr->lo = ylo; yr->hi = yhi;
  }

int st_map_nearest_vertex(Map *m, Point p)
  {
    int vi, imin;
    double d2min = MAXDOUBLE;
    for (vi = 0; vi < m->nv; vi++)
      { VertexData *vd = m->vd[vi]; 
        double dx = (p.c[0] - vd->p.c[0]);
        double dy = (p.c[1] - vd->p.c[1]);
        double d2 = dx*dx + dy*dy;
        if (d2 < d2min) { imin = vi; d2min = d2; }
      }
    return imin;
  }

/* INTERNAL PROCEDURES */
  
void st_map_file_error(int nlin, char *msg)
   /*
     Prints {msg} and {nlin}, and exits with error status.
     Handy while parsing a map file. */
  {
    fprintf(stderr, "map format error on line %d: %s\n", nlin, msg); 
    exit(1);
  }

void st_map_add_edge(Map *m, int org, int dst, int ei, float c0, float c1)
   /*
     Adds a new edge with number {ei} to the map's topology
     structure, between vertices {org} and {dst}, with costs {c0} and
     {c1} for forward and backwards traversal. Also stores the
     corresponding arcs in {m->out[org]}, {m->out[dst]} and
     {m->along[ei]}, and stores the edge's data record in {m->ed[ei]}.
     Assumes that the vertex data records {m->vd[...]} are already set. */
  {
    EdgeData *ed;
        
    quad_arc a0 = make_edge();
    quad_arc a1 = SYM(a0);
    int ia0 = 2*ei + SYMBIT(a0);
    int ia1 = 2*ei + SYMBIT(a1);
    
    affirm(org != dst, "loop edge");

    /* Set origin and destination data pointers: */
    ODATA(a0) = m->vd[org];
    ODATA(a1) = m->vd[dst];
    
    /* Create edge data record and set data pointers to it: */
    ed = (EdgeData *)malloc(sizeof(EdgeData));
    ed->id = ei;
    ed->cost[0] = c0;
    ed->cost[1] = c1;
    m->ed[ei] = ed;
    LDATA(a0) = ed;
    LDATA(a1) = ed;

    /* Connect to adjacent edges, and store arc in topology tables: */
    { bool ok0 = TRUE, ok1 = TRUE;
      if (m->out[org] != NULL_REF) 
        { ok0 = st_map_insert_edge_in_ring(a0, m->out[org]); }
      if (m->out[dst] != NULL_REF)
        { ok1 = st_map_insert_edge_in_ring(a1, m->out[dst]); }
      if (!(ok0 & ok1))
        { /* Detach this edge (a desperate attempt to keep going): */
          fprintf(stderr, "ring insertion failed for edge %d = (%d,%d)\n", ei,org,dst);
          splice(a0, OPREV(a0)); splice(a1, OPREV(a1));
        }
      else
        { m->out[org] = a0; m->along[ia0] = a0;
          m->out[dst] = a1; m->along[ia1] = a1;
        }
    }
  }

bool st_map_insert_edge_in_ring(quad_arc e, quad_arc r)
  /* 
    Inserts {e} in the proper place among the ring of edges out 
    of some vertex {v}, of which {r} is a representative.
    Returns {FALSE} if the insertion failed for some reason. */
  {
    VertexData *od = (VertexData *)ODATA(e);
    VertexData *ed = (VertexData *)DDATA(e);
    quad_arc a = r;
    bool here;
    
    affirm(ONEXT(e) == e, "edge ONEXT");
    do { 
      quad_arc b = ONEXT(a);
      if (b == a)
        { here = TRUE; }
      else
        { VertexData *ad = (VertexData *)DDATA(a);
          VertexData *bd = (VertexData *)DDATA(b);
          int sae, seb, sba;

          affirm(ODATA(a) == ODATA(e), "edge origin");
          sae = st_map_ccw(od->p, ad->p, ed->p);
          seb = st_map_ccw(od->p, ed->p, bd->p);
          sba = st_map_ccw(od->p, bd->p, ad->p);
          here = (sae + seb + sba > 0);
        }
      if (here) { splice(a, e); return TRUE; }
      a = b;
    } while (a != r);
    return FALSE;
  }
  
int st_map_ccw(Point p, Point q, Point r)
  /*
   Sign of triangle {p,q,r} (-1, 0, or +1). */
  {
    double ux = q.c[0] - p.c[0];
    double uy = q.c[1] - p.c[1];
    double vx = r.c[0] - p.c[0];
    double vy = r.c[1] - p.c[1];
    double det = ux*vy - uy*vx;
    if (det < 0) 
      { return -1; }
    else if (det > 0)
      { return 1; }
    else
      { return 0; }
  }

bool st_map_vertex_is_visible(Point p, Interval xr, Interval yr) 
   /* 
     Returns {TRUE} if the point {p} is within the box {xr × yr}. */
  { 
    return (p.c[0] >= xr.lo) & (p.c[0] <= xr.hi) & (p.c[1] >= yr.lo) & (p.c[1] <= yr.hi);
  }
  
double_vec_t st_read_double_vec_t(char *name)
  { affirm(name != NULL, "no file name given");
    affirm(strlen(name) != 0, "empty file name");
    FILE *rd = (strcmp(name, "-") == 0 ? stdin : open_read(name));
    double_vec_t d = st_fread_double_vec_t(rd);
    if (rd != stdin) { fclose(rd); }
    return d;
  }
  
double_vec_t st_fread_double_vec_t(FILE *rd)
  { int nel = nget_int(rd, "vertices"); fget_eol(rd);
    double_vec_t d = double_vec_new(nel);
    int i;
    for (i = 0; i < nel; i++) { d.el[i] = fget_double(rd); fget_eol(rd); }
    return d;
  }
  
void st_write_double_vec_t(char *name, double_vec_t *d)
  { affirm(name != NULL, "no file name given");
    affirm(strlen(name) != 0, "empty file name");
    FILE *wr = (strcmp(name, "-") == 0 ? stdout : open_write(name));
    st_fwrite_double_vec_t(wr, d);
    if (wr != stdout) { fclose(wr); }
  }

void st_fwrite_double_vec_t(FILE *wr, double_vec_t *d)
  { int i;
    fprintf(wr, "vertices = %d\n", d->nel);
    for (i = 0; i < d->nel; i++) { fprintf(wr, "%24.16e\n", d->el[i]); }
    fflush(wr);
  }

bool st_map_edge_is_visible(Point p, Point q, Interval xr, Interval yr) 
   /* 
     Returns {TRUE} if the bounding box of the segment {p--q}
     intercepts the box {xr × yr}. */
  { 
    if (p.c[0] < q.c[0])
      { if ((q.c[0] < xr.lo) || (p.c[0] > xr.hi)) { return FALSE; } }
    else
      { if ((p.c[0] < xr.lo) || (q.c[0] > xr.hi)) { return FALSE; } }
    if (p.c[1] < q.c[1])
      { if ((q.c[1] < yr.lo) || (p.c[1] > yr.hi)) { return FALSE; } }
    else
      { if ((p.c[1] < yr.lo) || (q.c[1] > yr.hi)) { return FALSE; } }
    return TRUE;
  }

void st_interval_widen(Interval *r, double margin)
  { r->lo = r->lo - margin;
    r->hi = r->hi + margin;
  }


void st_adjust_rect_shape(Interval *xr, Interval *yr, double tx, double ty)
  { double dx = xr->hi - xr->lo;
    double dy = yr->hi - yr->lo;
    if (ty*dx > tx*dy)
      { double ey = (ty*dx/tx - dy)/2.0;
        yr->lo = yr->lo - ey;
        yr->hi = yr->hi + ey;
      }
    else if (ty*dx < tx*dy)
      { double ex = (tx*dy/ty - dx)/2.0;
        xr->lo = xr->lo - ex;
        xr->hi = xr->hi + ex;
      }
  }