#include <iostream>
#include <string>
#include <list>
#include <fstream>
#include <sstream>
#include "serialport.h"
#include "wdigraph.h"
#include "dijkstra.h"
#include "server_util.h"

int main() {
  SerialPort Serial("/dev/ttyACM0");

  WDigraph graph;
  unordered_map<int, Point> points;
  string line;
  // char delimiter = " ";

    readGraph("edmonton-roads-2.0.1.txt", graph, points);
  while (true){
      do{
        line = Serial.readline(1000);
      } while(line[0] != 'R');
      //get the flag and erase it from string
    string flag = line.substr(0, line.find(" "));
    line.erase(0, line.find(" ") + 1);
    // Serial.writeline(line);

    // get the first latitude and erase it from string
    int first_lat = stoi(line.substr(0, line.find(" ")));
    line.erase(0, line.find(" ") + 1);

    // get the first longitude and erase it from string
    int first_lon = stoi(line.substr(0, line.find(" ")));
    line.erase(0, line.find(" ") + 1);

    // get the second latitude and erase it from string
    int second_lat = stoi(line.substr(0, line.find(" ")));
    line.erase(0, line.find(" ") + 1);

    // the rest is the second longitude
    int second_lon = stoi(line);

  // build the graph


  // read a request
  Point sPoint, ePoint;
  // cin >> c >> sPoint.lat >> sPoint.lon >> ePoint.lat >> ePoint.lon;
  sPoint.lat = static_cast<long long> (first_lat);
  sPoint.lon = static_cast<long long> (first_lon);
  ePoint.lat = static_cast<long long> (second_lat);
  ePoint.lon = static_cast<long long> (second_lon);

  // c will be 'R' in part 1, no need to check until part 2

  // get the points closest to the two input points
  int start = findClosest(sPoint, points), end = findClosest(ePoint, points);

  // run dijkstra's to compute a shortest path
  unordered_map<int, PLI> tree;
  dijkstra(graph, start, tree);

  if (tree.find(end) == tree.end()) {
      // no path
      // cout << "N 0" << endl;
    Serial.writeline("N 0\n");
  }
  else {
    // read off the path by stepping back through the search tree
    list<int> path;
    while (end != start) {
      path.push_front(end);
      end = tree[end].second;
    }
    path.push_front(start);

    // output the path
    // cout << "N " << path.size() << endl;
    string send = "N " + path.size();
    Serial.writeline(send);
    // Serial.writeline(path.size());
    Serial.writeline("\n");
    // line = Serial.readline(1000);

    for (auto v : path) {
      // cout << "W " << points[v].lat << ' ' << points[v].lon << endl;

    // send the position to the arduino
    string next_send = "W " + points[v].lat;
    Serial.writeline(next_send);
    Serial.writeline(" ");

    string next2_send = points[v].lon + "\n";
    Serial.writeline(next2_send);
    // Serial.writeline("W ");
    // Serial.writeline(points[v].lat);
    // Serial.writeline(" ");
    // Serial.writeline(points[v].lon);
    // Serial.writeline("\n");
    line = Serial.readline(1000);
    if (line[0] == 'A')
        continue;
    else {break;}
    }
    // cout << "E" << endl;
    // line = Serial.readline();
    Serial.writeline("E\n");
  }

  }

  return 0;
}