/* a-star-tester-2.cpp
   James S. Plank
   February, 2015 */

/* Now, I try to make the estimate better, by checking the distance to
   every node. I have a key bug in this program though. */

#include <string>
#include <sys/time.h>
#include <vector>
#include <list>
#include <cmath>
#include <algorithm>
#include <map>
#include <set>
#include <iostream>
#include <cstdio>
#include <cstdlib>
using namespace std;

#define VIT(i, v) for (i = 0; i < (int) v.size(); i++) 
#define IT(it, ds) for (it = ds.begin(); it != ds.end(); it++)
#define FUP(i, n) for (i = 0; i < n; i++)

/* I ripped this from my CS302 notes */

class Disjoint {
  public:
    Disjoint(int nelements);
    int Union(int s1, int s2);
    int Find(int element);
  protected:
    vector <int> links;
    vector <int> ranks;
};

Disjoint::Disjoint(int nelements)
{
  int i;

  links.resize(nelements);
  ranks.resize(nelements);
 
  for (i = 0; i < nelements; i++) {
    links[i] = -1;
    ranks[i] = 0;
  }
}

int Disjoint::Find(int element)
{
  int setid;

  if (links[element] == -1) {
    return element;
  }

  setid = Find(links[element]);
  links[element] = setid;
  return setid;
}

int Disjoint::Union(int s1, int s2)
{
  if (links[s1] != -1 || links[s2] != -1) {
    cerr << "Union called on a non-set\n";
    exit(1);
  }
  if (s1 == s2) return s1;
  if (ranks[s1] < ranks[s2]) {
    links[s1] = s2;
    return s1;
  } else if (ranks[s1] > ranks[s2]) {
    links[s2] = s1;
    return s2;
  } else {
    links[s2] = s1;
    ranks[s1]++;
    return s1;
  }
}

void usage(string s)
{
  fprintf(stderr, "usage: a-star-tester-0 seed xmin ymin xmax ymax nn connections-per-node Dijkstra|A-Star|Nothing print(Y|N|G)\n");
  if (s != "") fprintf(stderr, "%s\n", s.c_str());
  exit(1);
}

class Node {
  public:
    int id;                     /* Integer id of a node. */
    double X, Y;                /* X and Y coordinates on the plane */
    int Color;                  /* Color for drawing */

                                /* There are four adjacency lists (rather than one with structs */
    vector <Node *> Adj_N;      /* #1: The nodes on the adjacency lists */
    vector <double> Adj_D;      /* #2: The distance to Adj_N[i] */
    vector <int> Adj_BE_Index;  /* #3: Let t = n->Adj_N[i].  Then t->Adj_N[n->Adj_BE_Index[i]] = n */
    vector <int> Adj_Color;     /* #4: The color of the edge */

    multimap <double, Node *>::iterator xit, yit;  /* Where the node is on the xmap and ymap (see Graph) */

    Node *Backedge;             /* These are all for Dijkstra / A-Star */
    int Backedge_Index;
    double D, E, H, H2;
    multimap <double, Node *>::iterator qit;

    int considered;             /* This helps us set up the adjacency lists */
    void Print();
};

class Graph {
  public: 
    vector <Node *> Nodes;     /* The nodes, not including the sentinels. */
    Node *Sentinel_Low;        /* I have sentinels whose distances to the nodes is great. */
    Node *Sentinel_High;       /* I'm not really sure if I need them. */
    double SDist;            

    multimap <double, Node *> Xmap; /* These sort the nodes by x and y values, respectively */
    multimap <double, Node *> Ymap;

    Node *Start;               /* Starting and ending nodes for the shortest path calculation */
    Node *End;
    double Path_Length;

    Disjoint *D;               /* We want are graphs connected -- this helps with that determination. */
    int Num_Components;

    multimap <double, Node *> Best;  /* Setting up the adjacency lists is a little involved. */
    vector <Node *> Considered;      /* See the comments in the code for how this is done */

    string DAN;                      /* "Dijkstra" / "A-Star" / "Nothing */
    string Print;
    
    Node *Add_New_Node(double x, double y, int put_in_nodes); 
    void Add_Edge(Node *n, Node *n2);
    int Add_To_Best(Node *n, Node *n2, int num, char XY);
    void Do_Dijkstra();
    void Do_A_Star();
    void Reset();
};

double Distance(Node *n1, Node *n2)
{
  return sqrt((n1->X-n2->X)*(n1->X-n2->X)+(n1->Y-n2->Y)*(n1->Y-n2->Y));
}

void Node::Print()
{
  printf("NODE %d %.3lf %3lf\n", id, X, Y);
}

/* There are a few subtleties in adding an edge:
    1. If you add [n->t], then you need to add [t->n] too.
    2. You need to set up Adj_BE_Index for each node (see the Node description).
    3. You need to maintain connected components in the disjoint set
 */

void Graph::Add_Edge(Node *n, Node *n2)
{
  double d;
  int s1, s2;

  d = Distance(n, n2);
  n->Adj_BE_Index.push_back(n2->Adj_N.size());
  n2->Adj_BE_Index.push_back(n->Adj_N.size());
  n->Adj_D.push_back(d);
  n->Adj_N.push_back(n2);
  n2->Adj_D.push_back(d);
  n2->Adj_N.push_back(n);
  s1 = D->Find(n->id);
  s2 = D->Find(n2->id);
  if (s1 != s2) {
    D->Union(s1, s2);
    Num_Components--;
  }
}

/* There are very few subtleties in adding a new node. */

Node *Graph::Add_New_Node(double x, double y, int put_in_nodes)
{
  Node *n;

  n = new Node;
  n->considered = 0;

  if (put_in_nodes) {
    n->id = Nodes.size();
    Nodes.push_back(n);
  } else {
    n->id = -1;
  }
  n->X = x;
  n->Y = y;
  n->xit = Xmap.insert(make_pair(n->X, n));
  n->yit = Ymap.insert(make_pair(n->Y, n));
  return n;
}

/* Dijkstra's algorithm -- if you've taken CS302, you should be able to do this
   in your sleep */

void Graph::Do_Dijkstra()
{
  multimap <double, Node *> Q;
  Node *n, *t;
  double distance, nd;
  int i;

  Start->qit = Q.insert(make_pair(0, Start));
  Start->D = 0;
  Start->Color = 1;

  while (!Q.empty()) {
    distance = Q.begin()->first;
    n = Q.begin()->second;
    n->Color = 2;
    Q.erase(Q.begin());
    if (n == End) {
      Path_Length = distance;
      while (1) {
        t = n->Backedge;
        t->Color = 3;
        t->Adj_Color[n->Backedge_Index] = 4;
        n->Adj_Color[t->Adj_BE_Index[n->Backedge_Index]] = 4;
        if (t == Start) return;
        n = t;
      }
    }
    VIT(i, n->Adj_N) {
      t = n->Adj_N[i];
      n->Adj_Color[i] = 1;
      t->Adj_Color[n->Adj_BE_Index[i]] = 1;
      nd = distance + n->Adj_D[i];
      if (t->D == -1 || t->D > nd) {
        t->Color = 1;
        if (t->D != -1) Q.erase(t->qit);
        t->D = nd;
        t->qit = Q.insert(make_pair(nd, t));
        t->Backedge = n;
        t->Backedge_Index = i;
      }
    }
  }
}

/* A-Star is very much like Dijkstra, but it employs the estimate (H) as well 
   as the distance (D). */

void Graph::Do_A_Star()
{
  multimap <double, Node *> Q;
  Node *n, *t, *x;
  double distance, nd, ne, h;
  int i, j;

  Start->H = Distance(Start, End);
  Start->H2 = Start->H;
  Start->D = 0;
  Start->E = Start->H;
  Start->Color = 1;
  End->H = 0;
  End->H2 = 0;
  Start->qit = Q.insert(make_pair(Start->H, Start));

  while (!Q.empty()) {
    n = Q.begin()->second;
    distance = n->D;
    n->Color = 2;
    Q.erase(Q.begin());
    if (n == End) {
      Path_Length = distance;
      while (1) {
        t = n->Backedge;
        t->Color = 3;
        t->Adj_Color[n->Backedge_Index] = 4;
        n->Adj_Color[t->Adj_BE_Index[n->Backedge_Index]] = 4;
        if (t == Start) return;
        n = t;
      }
    }
    VIT(i, n->Adj_N) {
      t = n->Adj_N[i];
      if (t->H == -1) t->H = Distance(t, End);
      if (t->H2 == -1) {
        VIT(j, t->Adj_N) {
          x = t->Adj_N[j];
          if (x->Color != 2) {
            if (x->H == -1) x->H = Distance(x, End);
            h = (x->H2 == -1) ? x->H : x->H2;
            if (t->H2 == -1 || t->Adj_D[j] + h < t->H2) {
              t->H2 = t->Adj_D[j] + h;
            }
          }
        }
        if (t->H2 == -1) t->H2 = SDist;
      }
      n->Adj_Color[i] = 1;
      t->Adj_Color[n->Adj_BE_Index[i]] = 1;
      nd = distance + n->Adj_D[i];
      ne = nd + t->H2;
      if (t->E == -1 || t->E > ne) {
        t->Color = 1;
        if (t->E != -1) Q.erase(t->qit);
        t->D = nd;
        t->E = ne;
        t->qit = Q.insert(make_pair(ne, t));
        t->Backedge = n;
        t->Backedge_Index = i;
      }
    }
  }
}


/* This runs through all of the nodes, making sure that their defaults are set.
   This is the procedure I'd like to avoid calling, unless it's absolutely 
   necessary. */

void Graph::Reset()
{
  int i, j;
  Node *n;

  for (i = 0; i < (int) Nodes.size(); i++) {
    n = Nodes[i];
    n->Backedge = NULL;
    n->D = -1;
    n->E = -1;
    n->H = -1;
    n->H2 = -1;
    n->Color = 0;
    VIT(j, n->Adj_Color) n->Adj_Color[j] = 0;
  }
  Path_Length = 0;
}

/* See the description of setting up adjacency lists below in main() */

int Graph::Add_To_Best(Node *n, Node *n2, int num, char XY)
{
  double d;
  multimap <double, Node *>::iterator last;

  if (n2 == NULL) return 0;
  if (!n2->considered) {
    n2->considered = 1;
    Considered.push_back(n2);
    d = Distance(n, n2);
    Best.insert(make_pair(d, n2));
    while ((int) Best.size() > num) {
      last = Best.end();
      last--;
      Best.erase(last);
    }
  }
  if (n2 == Sentinel_Low || n2 == Sentinel_High) return 0;

  if ((int) Best.size() == num) {
    last = Best.end();
    last--;
    if (XY == 'X') {
      d = n2->X - n->X;
      if (d < 0) d = -d;
      if (d > last->first) return 0;
    } else {
      d = n2->Y - n->Y;
      if (d < 0) d = -d;
      if (d > last->first) return 0;
    }
  }
  return 1;
}  

  
int main(int argc, char **argv)
{
  double xmin, ymin, xmax, ymax;
  long seed;
  int nn, i, done, ntoadd, j, s1, nopen, ndefault;
  int cpn;
  Node *n, *n2;
  Node *xlow, *xhigh, *ylow, *yhigh;
  multimap <double, Node *>::iterator it, mit;
  Graph G;
  vector <string> colors;
  struct timeval tp;
  string s;
  double t1, t2, x, y;

  colors.push_back("1 1 1");
  colors.push_back("1 .5 .5");
  colors.push_back(".5 .5 1");
  colors.push_back("0 1 0");
  colors.push_back("1 1 0");

  if (argc != 10) usage("");

  if (sscanf(argv[1], "%ld", &seed) == 0) usage("Bad seed.");
  srand48(seed);
  if (sscanf(argv[2], "%lf", &xmin) == 0) usage("Bad xmin.");
  if (sscanf(argv[3], "%lf", &ymin) == 0) usage("Bad ymin.");
  if (sscanf(argv[4], "%lf", &xmax) == 0 || xmax <= xmin) usage("Bad xmax.");
  if (sscanf(argv[5], "%lf", &ymax) == 0 || ymax <= ymin) usage("Bad ymax.");
  if (sscanf(argv[6], "%d", &nn) == 0 || nn <= 0) usage("Bad nn.");
  if (sscanf(argv[7], "%d", &cpn) == 0) usage("Bad cpn.");
  G.DAN = argv[8];
  if (G.DAN != "Dijkstra" && G.DAN != "A-Star" && G.DAN != "Nothing") usage("Bad Dijkstra|A-Star|Nothing");
  G.Print = argv[9];
  if (G.Print != "Y" && G.Print != "N" && G.Print != "G") usage("Bad G.Print(Y|N|G)");

  /* Make two dummy nodes that couldn't possibly connect to anyone. */

  G.Sentinel_Low = G.Add_New_Node(xmin - (xmax - xmin)*2, ymin - (ymax - ymin)*2, 0);
  G.Sentinel_High = G.Add_New_Node(xmax + (xmax - xmin)*2, ymax + (ymax - ymin)*2, 0);
  G.SDist = Distance(G.Sentinel_Low, G.Sentinel_High);

  /* Set up the disjoint sets to count components */

  G.D = new Disjoint(nn);
  G.Num_Components = nn;
  
  /* Read the graph from standard input rather than make it */
  if (cpn <= 0) { 
    while (cin >> s) {
      if (s == "NODE") {
        if (!(cin >> i >> x >> y)) { fprintf(stderr, "Stdin in bad format (bad NODE)\n"); exit(1); }
        if (i != (int) G.Nodes.size()) { fprintf(stderr, "Bad NODE -- out of order (%d)\n", i); exit(1); }
        if (i == nn) { fprintf(stderr, "Too many nodes\n"); exit(1); }
        G.Add_New_Node(x, y, 1);
      } else if (s == "EDGE") {
        if (!(cin >> i >> j)) { fprintf(stderr, "Stdin in bad format (bad EDGE)\n"); exit(1); }
        if (i < 0 || i >= nn) { fprintf(stderr, "Bad edge id %d\n", i); exit(1); }
        if (j < 0 || j == nn) { fprintf(stderr, "Bad edge id %d\n", j); exit(1); }
        if (i > j) G.Add_Edge(G.Nodes[i], G.Nodes[j]);
      } else {
        fprintf(stderr, "Stdin in bad format (not a NODE or EDGE)\n");
        exit(0);
      }
    }

  /* Otherwise create the nodes randomly */

  } else {

    FUP(i, nn) {
      G.Add_New_Node(xmin + drand48() * (xmax - xmin), ymin + drand48() * (ymax - ymin), 1);
    }
  
    /* This is the tricky part.  My goal is to set up the nodes' adjacency lists.
       For each node, n, I am going to maintain two data structures:
         
          1. "Considered" is a vector of nodes that I have considered in this analysis.
             When I consider a node, I set its "considered" field.  I maintain "considered",
             because when I'm done setting up n's adjacency lists, I need to reset all of
             the "considered" fields to zero.  I don't want to run through all of the nodes
             in the graph to do this, so I keep track of them with the Considered vector.
  
          2. "Best" is a map of nodes that are not already on n's adjacency list, ordered by
             their distance to n.  I make sure that the size of Best is capped at the number
             of nodes that I want to put onto n's adjacency list.
  
       The first thing I do is set the considered fields of all of the nodes that are already
       on n's adjacency list. 
  
       Now, what I do is keep track of four nodes, xlow, xhigh, ylow and yhigh. 
       These start as the the nodes just before and after n xmap and the ymap.  For each 
       of these, if the node is not yet "considered," then I insert it into Best and set its
       considered field.  When I've done this for xlow, xhigh, ylow and yhigh, I set them to
       the previous/next nodes in the xmap and ymap.  I can stop considering each of these when:
  
          There are no more nodes on the xmap/ymap.
          The distance to n along the x or y axis is greater than the highest node on best.
  
       When I'm done, I put all of the nodes on best onto n's adjacency list (and vice versa, since
       our graph is undirected.)
    */
      
    IT(mit, G.Xmap) {
      n = mit->second;
      if (n->id >= 0) {
        it = n->xit; it--; xlow = it->second;
        it = n->xit; it++; xhigh = it->second;
        it = n->yit; it--; ylow = it->second;
        it = n->yit; it++; yhigh = it->second;
  
        if ((int) n->Adj_N.size() < cpn) {
  
          /* This sets the considered fields / vector for the nodes already on n's adjacency list */
  
          VIT(i, n->Adj_N) G.Add_To_Best(n, n->Adj_N[i], G.Best.size()+n->Adj_N.size(), 'X');
          G.Best.clear();
  
          /* This goes through the process described above for the four nodes */
  
          ntoadd = cpn - n->Adj_N.size(); 
          done = (ntoadd == 0);
          while (!done) {
            if (G.Add_To_Best(n, xlow, ntoadd, 'X') == 0) {
              xlow = NULL;
            } else {
              it = xlow->xit; it--; xlow = it->second;
            }
            if (G.Add_To_Best(n, xhigh, ntoadd, 'X') == 0) {
              xhigh = NULL;
            } else {
              it = xhigh->xit; it++; xhigh = it->second;
            }
            if (G.Add_To_Best(n, ylow, ntoadd, 'Y') == 0) {
              ylow = NULL;
            } else {
              it = ylow->yit; it--; ylow = it->second;
            }
            if (G.Add_To_Best(n, yhigh, ntoadd, 'Y') == 0) {
              yhigh = NULL;
            } else {
              it = yhigh->yit; it++; yhigh = it->second;
            }
            done = (xlow == NULL && xhigh == NULL && ylow == NULL && yhigh == NULL);
          }
  
          /* Now, we add all of the edges to the adjacency lists. */
  
          IT(it, G.Best) G.Add_Edge(n, it->second);
   
          /* And clear all of the considered fields */
  
          VIT(i, G.Considered) G.Considered[i]->considered = 0;
          G.Considered.clear();
        }
      }
    }
  
    /* Our last step is to connect the graph.  We've been using the disjoint set to
       keep track of connected components.
  
       To connect the graph, pick a random node.  Find the closest node to that one
       that is not in the same segment.  Then find the closest node to *that* one
       that is not in the same segment. That should be O(|N|) for each component. */
  
    while (G.Num_Components > 1) {
      n2 = NULL;
      n = G.Nodes[lrand48()%G.Nodes.size()];
      s1 = G.D->Find(n->id);
      VIT(i, G.Nodes) {
        if (G.D->Find(G.Nodes[i]->id) != s1) {
          if (n2 == NULL || Distance(n, G.Nodes[i]) < Distance(n, n2)) n2 = G.Nodes[i];
        }
      }
      n = n2;
      n2 = NULL;
      s1 = G.D->Find(n->id);
      VIT(i, G.Nodes) {
        if (G.D->Find(G.Nodes[i]->id) != s1) {
          if (n2 == NULL || Distance(n, G.Nodes[i]) < Distance(n, n2)) n2 = G.Nodes[i];
        }
      }
      G.Add_Edge(n, n2);
    } 
  }

  /* Set all colors to the default color */

  VIT(i, G.Nodes) {
    n = G.Nodes[i];
    n->Color = 0;
    n->Adj_Color.resize(n->Adj_N.size(), 0);
  }

  /* Set the starting node to be the one 20% in on the x axis, and the
     ending node to be the one 80% in on the x axis. */

  it = G.Xmap.begin();
  it++;
  for (i = 0; i < (int) G.Nodes.size()*2/10; i++) it++;
  G.Start = it->second;
  for (i = 0; i < (int) G.Nodes.size()*6/10; i++) it++;
  G.End = it->second;
  G.Reset();

  /* Do the respective algorithm. */

  gettimeofday(&tp, NULL);
  t1 = tp.tv_usec;
  t1 /= 1000000.0;
  t1 += tp.tv_sec;

  if (G.DAN == "Nothing") {
  } else if (G.DAN == "Dijkstra") {
    G.Do_Dijkstra();   
  } else if (G.DAN == "A-Star") {
    G.Do_A_Star();   
  }

  gettimeofday(&tp, NULL);
  t2 = tp.tv_usec;
  t2 /= 1000000.0;
  t2 += tp.tv_sec;

  /* And print if need be */

  G.Start->Color = 4;
  G.End->Color = 4;

  if (G.Print == "Y") {
    printf("newgraph\n");
    printf("xaxis min %.3lf max %3lf size 8 nodraw\n", xmin, xmax);
    printf("yaxis min %.3lf max %3lf size 8 nodraw\n", ymin, ymax);

    printf("newcurve marktype box gray 0 marksize %.3lf %.3lf pts %.3lf %.3lf\n",
       (xmax-xmin)*1.05, (xmax-ymin)*1.05,  (xmax+xmin)/2.0, (ymax+ymin)/2.0);
    VIT(i, G.Nodes) {
      n = G.Nodes[i];
      VIT(j, n->Adj_N) { 
        if (n->id > n->Adj_N[j]->id) { 
          printf("newline color %s pts %.3lf %.3lf %.3lf %.3lf\n", 
             colors[n->Adj_Color[j]].c_str(), 
             n->X, n->Y, n->Adj_N[j]->X, n->Adj_N[j]->Y);
          if (n->Adj_Color[j] == 4) printf("linethickness 2\n");
        } 
      }
    }
  
    VIT(i, G.Nodes) {
      printf("newcurve marktype circle color %s pts %.3lf %.3lf\n", 
        colors[G.Nodes[i]->Color].c_str(), G.Nodes[i]->X, G.Nodes[i]->Y);
    }
    printf("newcurve marktype circle color %s pts %.3lf %.3lf\n", 
      colors[G.Start->Color].c_str(), G.Start->X, G.Start->Y);
    printf("newcurve marktype circle color %s pts %.3lf %.3lf\n", 
      colors[G.End->Color].c_str(), G.End->X, G.End->Y);
  } else if (G.Print == "G") {
    VIT(i, G.Nodes) {
      n = G.Nodes[i];
      printf("NODE %d %lg %lg\n", n->id, n->X, n->Y);
    }
    VIT(i, G.Nodes) {
      n = G.Nodes[i];
      VIT(j, n->Adj_N) {
        printf("EDGE %d %d\n", n->id, n->Adj_N[j]->id);
      }
    }
    exit(0);
  }
  
  printf("(* Time:         %9.6lf *)\n", t2-t1);
  nopen = 0;
  ndefault = 0;
  VIT(i, G.Nodes) {
    if (G.Nodes[i]->Color == 0) ndefault++;
    if (G.Nodes[i]->Color == 1) nopen++;
  }
  printf("(* Path Length:     %6.3lf *)\n", G.Path_Length);
  printf("(* Closed Set Size: %6d *)\n", (int) G.Nodes.size() - ndefault - nopen);
  printf("(* Open   Set Size: %6d *)\n", nopen);
  printf("(* Unvisited Nodes: %6d *)\n", ndefault);

  return 0;
}