/*
  Main program file for IRA Assignment 1.  Fill in the blanks with
  your own code.
*/

#include <dolt-lite.h>
#include <stdio.h>
#include <vector>
#include "input.h"
#include "intersection.h"

using namespace std;
using namespace dolt;

/* global variables: */

Graphics g;     // dolt graphics object
World    world; // world (boundary + obstacles)
Problem  prob;  // problem specification (parameters)

// object lists for drawing different aspects of the PRM
objectList world_list, sample_list, graph_list, path_list;

vector<drawablePoint> V; // sampled robot configurations



/********************************************************************/
/* you need to implement the following functions:                   */
/********************************************************************/

/*
  This function performs step 1 of the PRM algorithm: sample N
  collision-free robot configuration points.
 */
void sample_points()
{
  // First, destroy any previous samples/graphs/paths
  // NOTE: you will need to update this to clean up your graphs and
  // paths, depending on the representation you choose.
  V.clear();
  sample_list.clear();

  if (prob.method() == Problem::random)
    cout << "Using random sampling..." << endl;
  else if (prob.method() == Problem::quasirandom)
    cout << "Using quasirandom sampling..." << endl;

  // This example deterministically picks a point and creates a
  // polygon representing a triangular robot centered at the point.
  // You will instead need to create an approximately circular robot.
  Point p(2.0, 2.0);
  Polygon poly;
  poly.push_back(p + Point(0.0, 0.5));
  poly.push_back(p + Point(-0.3, -0.3));
  poly.push_back(p + Point(0.3, -0.3));

  // You will need to test whether the robot intersects with the
  // world.

  // Let's assume it doesn't.  Then we add the configuration to the
  // list of valid samples.
  V.push_back(p); // this uses p to initialize a "drawable point"

  // You will of course repeat the process N times.  (Note that N is
  // available as prob.N()).  When you're done sampling points you
  // will want to add them all to the set of things to be drawn:
  for(unsigned int i = 0; i < V.size(); ++i)
    sample_list.push_back(&V[i]);

  //
  // Here are some other examples you might find useful:
  //

  // Accessing the world bounding box:
  Bbox b = world.boundary().bbox();
  // Getting values for the bounding box extent:
  double xmin = b.xmin();  double ymin = b.ymin();
  double xmax = b.xmax();  double ymax = b.ymax();

  // Accessing the obstacles from the world:
  list<drawablePolygon>::const_iterator i;
  for(i = world.obstacles().begin(); i != world.obstacles().end(); ++i) {
    // Accessing the edges of the obstacle:
    drawablePolygon::Edge_const_iterator e;
    for(e = i->edges_begin(); e != i->edges_end(); ++e) {
      // *e is a dolt::Segment:
      Point start = e->start();
      Point end = e->end();
    }
  }

  // Accessing the N and k parameters from the problem file:
  unsigned int N = prob.N();
  unsigned int k = prob.k();

  // Intersecting two line segments:
  Segment s1(Point(0.0, 5.0), Point(10.0, 5.0));
  Segment s2(Point(5.0, 0.0), Point(5.0, 10.0));
  if(test_segmentIntersect(s1, s2))
    cout << "They intersect!" << endl;
  else
    cout << "Kris made a mistake, blame him!" << endl;
}

void make_graph()
{
  // First, destroy any previous graphs/paths.
  // Clean up whatever graph representation you're using.
  graph_list.clear();

  // Then, build a graph (using whatever representation you choose)
  // from the sampled points.  You may find it convenient to construct
  // a graph where each edge is represented by or is associated with a
  // "drawableSegment."  Here is how to create a drawableSegment that
  // starts at (0,0), ends at (2,2), has a 2 pixel width, is solid
  // (not dashed), and is colored blue:
  drawableSegment s(Point(0.0, 0.0), Point(2.0, 2.0), 2, SOLID, "blue");

  // You can find out more about drawableSegments (and all other
  // drawableObjects) by looking at drawable.h in the dolt-lite
  // include directory, or by looking at the dolt.html that comes with
  // dolt-lite.

  // You will want to add the drawableSegments from your graph to the
  // "graph_list" objectList.  (Alternatively, you can derive your
  // graph representation from drawableLineObject and define the
  // drawGL and drawPS functions, and then add the graph object to
  // graph_list instead.)
}

void find_path()
{
  // First, destroy any previous paths.
  // Clean up whatever path representation you're using.
  path_list.clear();

  // Then, add the start and goal configurations to your graph.

  // Then, build a path by searching the graph using A*.  You may want
  // to represent your path as a "drawableLineStrip."  Here is how to
  // create a drawableLineStrip with 3 points, a 5 pixel width, solid,
  // red, and partially transparent:
  drawableLineStrip s(5, SOLID, Color(1.0 /* R */, 0.0 /* G */, 0.0 /* B */, 0.5 /* alpha */));
  s.push_back(Point(0.0, 0.0));
  s.push_back(Point(2.0, 2.0));
  s.push_back(Point(1.0, 0.0));

  // You will find it useful to get the start and goal configurations
  // from the robot path:
  Point start = prob.start();
  Point end = prob.goal();
}

/********************************************************************/
/* the rest should be fine as-is, but feel free to change it        */
/********************************************************************/

/* keyboard handler */
void keyboard(unsigned char key)
{
  switch(key) {
  case 'q':
  case 'Q': // quit
    exit(0);

  case 's':
  case 'S': // sample N collision-free robot configuration points
    sample_points();
    break;

  case 'g':
  case 'G': // make a graph from the sampled points
    make_graph();
    break;

  case 'p':
  case 'P': // find a path through the graph between the start and goal
    find_path();
    break;

  case 'w':
  case 'W': // show/hide the world
    world_list.toggle_visibility();
    break;
  case 'a':
  case 'A': // show/hide the sampled points
    sample_list.toggle_visibility();
    break;
  case 'r':
  case 'R': // show/hide the graph
    graph_list.toggle_visibility();
    break;
  case 't':
  case 'T': // show/hide the path
    path_list.toggle_visibility();
    break;

  case 'h':
  case 'H':
  default:
    printf("Commands:\n q: Quit\n h: Help\n"
	   " s: sample N collision-free robot configuration points\n"
	   " g: make a graph from the sampled points\n"
	   " p: find a path through the graph between the start and goal\n");
    printf("\n w or W: show/hide the World\n"
	   " a or A: show/hide the sAmpled points\n"
	   " r or R: show/hide the gRaph\n"
	   " t or T: show/hide the paTh\n\n");
    g.printControlKeyBindings();
    break;
  }
}

/* main program */
int main(int argc, char **argv)
{
  if(argc < 2) {
    printf("Usage: %s <problem file>\n", argv[0]);
    return 1;
  }

  if(!prob.read_file(argv[1], world)) {
    printf("Can't read problem file %s!\n", argv[1]);
    return 1;
  }
  world_list.push_back(&world);

  double w = world.boundary().bbox().xmax() - world.boundary().bbox().xmin();
  double h = world.boundary().bbox().ymax() - world.boundary().bbox().ymin();
  unsigned int pw = 800, ph = 800;
  if(w > h)
    ph = (unsigned int)(800 * h / w);
  else
    pw = (unsigned int)(800 * w / h);

  g.initGL(&argc, argv, "IRA Assignment 1", pw, ph); // initialize dolt window
  g.setWorldBounds(world.boundary().bbox());
  g.setKeyboardHandler(keyboard); // register a keyboard callback function

  // tell dolt to draw the objects in each of these lists; the first
  // list is drawn on the bottom, the last on the top
  g.addObjectList(world_list);
  g.addObjectList(sample_list);
  g.addObjectList(graph_list);
  g.addObjectList(path_list);

  // make points a little bigger and green by default
  drawablePoint::setDefaultSize(6);
  drawablePoint::setDefaultObjectColor("green");

  g.runEventLoop(); // run the main loop

  return 0;
}