//===========================================================================
// Copyright (C) 1998, 2000-2007, 2010, 2011, 2012 SINTEF ICT, Applied
// Mathematics, Norway.
//
// This file is part of GoTools
//
// This program is free software; you can redistribute it and/or          
// modify it under the terms of the GNU General Public License            
// as published by the Free Software Foundation version 2 of the License. 
//
// This program is distributed in the hope that it will be useful,        
// but WITHOUT ANY WARRANTY; without even the implied warranty of         
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          
// GNU General Public License for more details.                           
//
// You should have received a copy of the GNU General Public License      
// along with this program; if not, see <http://www.gnu.org/licenses>
//
// Contact information: E-mail: tor.dokken@sintef.no                      
// SINTEF ICT, Department of Applied Mathematics,                         
// P.O. Box 124 Blindern,                                                 
// 0314 Oslo, Norway.                                                     
//
// Other licenses are also available for this software, notably licenses
// for:
// - Building commercial software.                                        
// - Building software whose source code you wish to keep private.        
//===========================================================================

#include "GoTools/parametrization/PrMultiDijkstra.h"
#include <assert.h>
//#include <limits>

//-----------------------------------------------------------------------------
MultiDijkstra::MultiDijkstra() 
//-----------------------------------------------------------------------------
{
  large_distance_ = 1e100; //(double)std::numeric_limits<double>::infinity();
  queue_      = new HeapType2;
  distances_  = new vector<double>;
  back_trace_ = new vector<int>;
  label_      = new vector<int>;
  flags_      = new vector<int>;
  graph_      = NULL;
}


//-----------------------------------------------------------------------------
MultiDijkstra::~MultiDijkstra() 
//-----------------------------------------------------------------------------
{
  if(queue_)      delete queue_;
  if(distances_)  delete distances_;
  if(flags_)      delete flags_;
  if(back_trace_) delete back_trace_;
  if(label_)      delete label_;
}



//-----------------------------------------------------------------------------
void MultiDijkstra::initialize() 
//-----------------------------------------------------------------------------
{
  int i=0;

  if (graph_ == NULL) {
    assert(graph_ != NULL);
    return;
  }

  int size = graph_->getNumNodes();
  // queue_->reserve(size);
  // queue_->empty();
  if (queue_) delete queue_;
  queue_ = new HeapType2;
  
  distances_->reserve(size);
  back_trace_->reserve(size);
  label_->reserve(size);
  flags_->reserve(size);

  
  for (i=0; i<size; i++) {
    (*distances_)[i]  = large_distance_;
    (*flags_)[i]      = 0;
    (*back_trace_)[i] = -1;
    (*label_)[i]      = -1;
  }

}



//-----------------------------------------------------------------------------
void MultiDijkstra::setSource( int node_idx, int node_label, double dist)
//-----------------------------------------------------------------------------
{
  (*distances_)[node_idx] = dist;
  (*label_)[node_idx] = node_label;
  // setFinished(node_idx);
  insertInCandidates(node_idx);
}




//-----------------------------------------------------------------------------
void MultiDijkstra::run()
//-----------------------------------------------------------------------------
{
  vector<int>  neighbours;
  double dist,currdist,nextdist;
  int curr,next;
  int i;
  int size = graph_->getNumNodes();
  
  // reset "visited" flags to zero 
  for (i=0; i<size; i++) {
    (*flags_)[i]      = 0;
  }
  
  // run the algorithms
  while (!queue_->empty()) {
    curr     = queue_->top().idx_; 
    queue_->pop();
    while(isFinished(curr) && !queue_->empty()) {
      curr     = queue_->top().idx_; 
      queue_->pop();
    }

    // Return if node is finished, i.e. queue is empty 
    if (isFinished(curr)) {
      return ;
    }

    currdist = (*distances_)[curr];


    setFinished(curr);
    graph_->getNeighbours(curr,neighbours);
    
    int s = (int)neighbours.size();
    for ( i=0; i<s; i++) {
      next = neighbours[i];
      if (!isFinished(next)){
        dist = getDistance(curr,next);
        nextdist = currdist+dist;
        if (nextdist < (*distances_)[next]) {
          (*distances_)[next] = nextdist;
	  (*label_)[next] = (*label_)[curr];
          insertInCandidates(next);
        } // if
      } // if
    } // for
  } // while
}


//-----------------------------------------------------------------------------
void MultiDijkstra::run( double radius)
//-----------------------------------------------------------------------------
{
  vector<int>  neighbours;
  double dist,currdist,nextdist;
  int curr,next;
  int i;
  
  
  while (!queue_->empty()) {
    curr     = queue_->top().idx_; 
    queue_->pop();
    while(isFinished(curr) && !queue_->empty()) {
      curr     = queue_->top().idx_; 
      queue_->pop();
    }

    // Return if node is finished, i.e. queue is empty 
    if (isFinished(curr)) {
      return ;
    }

    currdist = (*distances_)[curr];
    setFinished(curr);
    if (currdist>=radius)
      continue;
    
    graph_->getNeighbours(curr,neighbours);
    
    int s = (int)neighbours.size();
    for ( i=0; i<s; i++) {
      next = neighbours[i];
      if (!isFinished(next)){
        dist = getDistance(curr,next);
        nextdist = currdist+dist;
        if (nextdist < (*distances_)[next]) {
          (*distances_)[next] = nextdist;
          insertInCandidates(next);
        } // if
      } // if
    } // for
  } // while
}

//-----------------------------------------------------------------------------
void    MultiDijkstra::run( int  target)
//-----------------------------------------------------------------------------
{
  vector<int>  neighbours;
  double dist,currdist,nextdist;
  int curr,next;
  int i;

  
  
  while (!queue_->empty()) {
    curr     = queue_->top().idx_; 
    queue_->pop();
    while(isFinished(curr) && !queue_->empty()) {
      curr     = queue_->top().idx_; 
      queue_->pop();
    }

    // Return if node is finished, i.e. queue is empty 
    if (isFinished(curr)) {
      return ;
    }

    currdist = (*distances_)[curr];
    setFinished(curr);

    // Finished
    if (curr == target)
      return;
    
    graph_->getNeighbours(curr,neighbours);
    
    int s = (int)neighbours.size();
    for ( i=0; i<s; i++) {
      next = neighbours[i];
      if (!isFinished(next)){
        dist = getDistance(curr,next);
        nextdist = currdist+dist;
        if (nextdist < (*distances_)[next]) {
          (*distances_)[next] = nextdist;
	  (*back_trace_)[next] = curr;
          insertInCandidates(next);
        } // if
      } // if
    } // for
  } // while
}


//-----------------------------------------------------------------------------
int MultiDijkstra::closestNeighbour(int node_idx)
//-----------------------------------------------------------------------------
{
  if ((*back_trace_)[node_idx] == -1) {
    std::cerr << "!!! PrMultiDijkstra: backtrace didn't work !!!" << std::endl;
    return (0);
  }

  return (*back_trace_)[node_idx];

  // bad code !!!
/*
  vector<int> neighbours;
  graph_->getNeighbours(node_idx,neighbours);

  if (neighbours.size()==0)
    return -1;

  double closest_dist = getDistance(neighbours[0]);
  int    closest_node = neighbours[0];
  for (int i=1; i<neighbours.size();i++) {
    int    node = neighbours[i];
    double dist = getDistance(node);
    
    if (fabs (dist-closest_dist) < 0.0000000001)
      cerr << "!!! PrDijkstr: two paths with same length !!!" << endl;

    if (dist<closest_dist) {
      closest_node = node;
      closest_dist = dist;
    }
  }
  return closest_node;
*/
}




