#include <iostream>
#include "shortest_path.h"
#include "location.h"

node_t::node_t(node_t* next, location_t* location, node_t* from, double from_origin, double total)
  : next{next}, location{location} {
  update(from, from_origin, total);
}

void node_t::update(node_t* from, double from_origin, double total) {
  this->from = from;
  this->from_origin = from_origin;
  this->total = total;
  this->visited = false;
}

void shortest_path_t::compute() {
  node_t* node;

  head = new node_t { nullptr, from, nullptr, 0, from->distance_to(to) };
  base = head;

  while(base != nullptr && base->location != to) {
    base->visited = true;

    for(size_t i = 0; i < base->location->nb_neighbors; i++) {
      location_t* cur = base->location->neighbors[i];
      double from_origin = base->from_origin + base->location->distance_to(cur);
      double total = from_origin + cur->distance_to(to);

      for(node = head; node != nullptr && node->location != cur; node = node->next) { }
      
      if(node == nullptr)
        head = new node_t { head, cur, base, from_origin, total };
      else if(from_origin < node->from_origin)
        node->update(base, from_origin, total);
    }

    base = nullptr;
    for(node = head; node != nullptr; node = node->next)
      if(!node->visited && (!base || node->total < base->total))
        base = node;
  }
}

void shortest_path_t::display_rec(node_t* cur) {
  if(cur->from != nullptr)
    display_rec(cur->from);
  
  std::cout << "  * " << cur->location->name << " at " << cur->from_origin << " km from " << from->name << std::endl;
}

void shortest_path_t::display() {
  if(base == nullptr) 
    std::cout << "unable to find a path from " << from->name << " to " << to->name << std::endl;
  else {
    std::cout << "path from " << from->name << " to " << to->name << std::endl;
    display_rec(base);
  }
}

shortest_path_t::~shortest_path_t() {
  while(head != nullptr) {
    node_t* next = head->next;
    delete head;
    head = next;
  }
}