int main(int argc, char** argv)
{
  // Check arguments
  if (argc < 3) {
    std::cerr << "Usage: " << argv[0] << " NODES_FILE TETS_FILE\n";
    exit(1);
  }

  // Construct a Graph
  typedef Graph<int, int> GraphType;
  GraphType graph;
  std::vector<GraphType::node_type> nodes;

  // Create a nodes_file from the first input argument
  std::ifstream nodes_file(argv[1]);
  // Interpret each line of the nodes_file as a 3D Point and add to the Graph
  Point p;
  while (CME212::getline_parsed(nodes_file, p))
    nodes.push_back(graph.add_node(p));

  // Create a tets_file from the second input argument
  std::ifstream tets_file(argv[2]);
  // Interpret each line of the tets_file as four ints which refer to nodes
  std::array<int,4> t;
  while (CME212::getline_parsed(tets_file, t))
    for (unsigned i = 1; i < t.size(); ++i)
      for (unsigned j = 0; j < i; ++j)
        graph.add_edge(nodes[t[i]], nodes[t[j]]);

  // Print out the stats
  std::cout << graph.num_nodes() << " " << graph.num_edges() << std::endl;

  // Launch the SDLViewer
  CME212::SDLViewer viewer;
  viewer.launch();
  auto node_map = viewer.empty_node_map(graph);

  Point pref = Point(-1, 0, 1);
  int path = shortest_path_lengths(graph, pref);
  PathColorFn pcf = PathColorFn(path);
  viewer.add_nodes(graph.node_begin(), graph.node_end(), pcf, node_map);

  // Test the PositionColorFn, the color is presented according to the nodes' x coordinats.
  //PositionColorFn pocf = PositionColorFn();
  //viewer.add_nodes(graph.node_begin(), graph.node_end(), pocf, node_map);

  viewer.add_edges(graph.edge_begin(), graph.edge_end(), node_map);
  viewer.center_view();

  return 0;
}
// =============================================================================
int main(int argc, char** argv) {
  // Check arguments
  if (argc < 3) {
    std::cerr << "Usage: " << argv[0] << " NODES_FILE TETS_FILE\n";
    exit(1);
  }

  // Construct an empty graph
  GraphType graph;

  // Create a nodes_file from the first input argument
  std::ifstream nodes_file(argv[1]);
  // Interpret each line of the nodes_file as a 3D Point and add to the Graph
  Point p;
  std::vector<typename GraphType::node_type> nodes;
  while (CME212::getline_parsed(nodes_file, p))
  nodes.push_back(graph.add_node(p));

  // Create a tets_file from the second input argument
  std::ifstream tets_file(argv[2]);
  // Interpret each line of the tets_file as four ints which refer to nodes
  std::array<int,4> t;
  while (CME212::getline_parsed(tets_file, t)) {
    graph.add_edge(nodes[t[0]], nodes[t[1]]);
    graph.add_edge(nodes[t[0]], nodes[t[2]]);
    #if 1
    // Diagonal edges
    graph.add_edge(nodes[t[0]], nodes[t[3]]);
    graph.add_edge(nodes[t[1]], nodes[t[2]]);
    #endif
    graph.add_edge(nodes[t[1]], nodes[t[3]]);
    graph.add_edge(nodes[t[2]], nodes[t[3]]);
  }



  // Set initial velocity and mass
  for (GraphType::NodeIterator it = graph.node_begin(); it != graph.node_end(); ++it) {
    Node n = *it;
    n.value().vel = Point(0,0,0);       // Initial velocity == 0
    n.value().mass = 1.0 / graph.num_nodes(); // graph has total mass == 1, constant density
  }

  // Set rest length for all of the Edges to their initial length
  for (GraphType::EdgeIterator ei = graph.edge_begin(); ei != graph.edge_end(); ++ei ) {
    (*ei).value().L = (*ei).length();
  }

  // Print out the stats
  std::cout << graph.num_nodes() << " " << graph.num_edges() << std::endl;

  // Launch the SDLViewer
  CME212::SDLViewer viewer;
  auto node_map = viewer.empty_node_map(graph);
  viewer.launch();

  viewer.add_nodes(graph.node_begin(), graph.node_end(), node_map);
  viewer.add_edges(graph.edge_begin(), graph.edge_end(), node_map);

  viewer.center_view();

  // Begin the mass-spring simulation
  double dt = 0.001;
  double t_start = 0;
  double t_end = 5.0;

  for (double t = t_start; t < t_end; t += dt) {
    // P1 ---------------------------------------------------------------------
    // symp_euler_step(graph, t, dt, Problem1Force());

    // P3 ----------------------------------------------------------------------
    //symp_euler_step(graph, t, dt, cf);

    // Create individual forces
    GravityForce g(grav);
    MassSpringForce msf(100);
    DampingForce d(1.0 / graph.num_nodes());

    // Combine the individual forces
    auto cf = make_combined_force(g, msf, d);

    // P4 ----------------------------------------------------------------------
    // Create individual constraints
    HPlane hp(-0.75);
    Sphere sp(0.15, Point(0.5,0.5,-0.5));
    SphereRemove sr(0.15, Point(0.5,0.5,-0.5));

    // Combined individual constraints
    // P4.1
    // auto c = make_combined_constraint(hp, FixedConstraint());
    // P4.2
    // auto c = make_combined_constraint(sp, FixedConstraint());
    // P4.3
    auto c = make_combined_constraint(sr, FixedConstraint());
    // Mixed constraints
    // auto c = make_combined_constraint(hp, sr, FixedConstraint());
    // auto c = make_combined_constraint(hp, sp, FixedConstraint());

    symp_euler_step(graph, t, dt, cf, c);

    viewer.clear();
    node_map.clear();
    viewer.add_nodes(graph.node_begin(), graph.node_end(), node_map);
    viewer.add_edges(graph.edge_begin(), graph.edge_end(), node_map);

    // Update viewer with nodes' new positions
    viewer.add_nodes(graph.node_begin(), graph.node_end(), node_map);
    viewer.set_label(t);

    // These lines slow down the animation for small graphs, like grid0_*.
    // Feel free to remove them or tweak the constants.
    if (graph.size() < 100)
    CME212::sleep(0.0001);
  }

  return 0;
}