/*   
 *   A-Star.cpp Copyright (C) 2013 Paolo Medici
 *
 *   This library is free software; you can redistribute it and/or modify it under the terms of the 
 *    GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of 
 *    the License, or (at your option) any later version.
 *
 *   This library 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 Lesser General Public 
 *    License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License along with this library; if not, 
 *   write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 
 */


#include <vector>
#include <map>
#include <cmath>
#include <limits>
#include <iostream>
#include <cassert>


////////////////

#define NUM_OF_DIRECTIONS       8

/// DIRECTIONS
static const int directions_x[NUM_OF_DIRECTIONS] = {0,1,1,1,0,-1,-1,-1};
static const int directions_y[NUM_OF_DIRECTIONS] = {-1,-1,0,1,1,1,0,-1};

// "Euristic"
float H(const std::pair<int, int> & a, const std::pair<int,int> & b)
{
  return std::sqrt( std::pow(a.first - b.first,2) + std::pow(a.second - b.second,2) );
}

bool find_path(std::vector< std::pair<int, int> > & out, const bool *wall, unsigned int width, unsigned int height, std::pair<int,int> start, std::pair<int,int> goal)
{
  assert (width>0 && height>0 && width<65536 && height<65536); // bounds
  assert (wall != 0 ); // invalid parameters
  
  float *g = new float[width * height]; // travel cost map
  int *path = new int[width * height];  // travelling directions map
  std::multimap<float, std::pair<int,int> > openset; // the open set
  float delta[NUM_OF_DIRECTIONS]; // precomputed traversing cost
  
  // precompute trasversing cost
  for(int i =0;i<NUM_OF_DIRECTIONS;++i)
    delta[i] = std::sqrt( directions_x[i]*directions_x[i] + directions_y[i]*directions_y[i] );
  
  // initialize path and g
  for(int i =0;i<width * height;++i)
  {
    //     g[i] = wall[i] ? 0.0f : std::numeric_limits<float>::max() // could be used as alternative
    g[i] = std::numeric_limits<float>::max(); // 
    path[i] = -1; 
  }
  
  // set the starting point
  g[ start.first + start.second * width] = 0.0;
  float f = H(start, goal);
  openset.insert (std::pair<float, std::pair<int,int> >(f, start));
  
  // iterate until openset is empty
  while(!openset.empty())
  {
    // get the element with lower f using multimap
    std::multimap<float, std::pair<int,int> >::iterator it=openset.begin();
    float f = it->first;
    std::pair<int,int> current = it->second;
    openset.erase(it);
    
    // is it the goal?
    if(current == goal)
    {
      // reconstruct path
      std::pair<int, int> cur = goal;
      out.push_back(cur);
      while(start != cur)
      {
        int d = path[cur.first + cur.second * width];
        if(d == -1)
        {
          // INTERNAL ERROR
          delete [] g;
          delete [] path;
          
          return false;
        }
        cur.first = d % width;
        cur.second = d / width;
        
        out.push_back( cur );
      }
      
      delete [] g;
      delete [] path;
      
      return true;
    }
    
    // search on neighbours
    for(int i=0;i<NUM_OF_DIRECTIONS;++i)
    {
      std::pair<int, int> n(current.first + directions_x[i], current.second + directions_y[i]);
      
      // evaluate point inside map and not walled
      if(n.first>=0 && n.second>=0 && n.first<width && n.second<height && (!wall[ n.first + n.second * width ]) )
      {
        // update travel cost (g)
        float t_g = g[current.first + current.second * width] + delta[i];
        
        if(t_g < g[n.first + n.second * width])
        {
          // compute expected cost to goal (f)
          float t_f = t_g + H(n, goal);
          g[n.first + n.second * width] = t_g; // update travel cost
          path[n.first + n.second * width] = current.first + current.second * width; // store coming path
          openset.insert (std::pair<float, std::pair<int,int> >(t_f, n)); // put this path on openset
        }
        
      }
    }
    
    
    
  }  
  
  delete [] g;
  delete [] path;
  
  return false;
  
}


//////////////// TEST APP /////////////////////

int main()
{
  std::vector< std::pair<int, int> > path;
  
  // an example of wall-map
  bool wall[8*8]={0,0,0,0,0,0,0,0,
                  0,0,1,1,1,1,0,0,
                  0,0,1,0,0,1,1,1,
                  0,0,1,0,0,0,0,0,
                  0,0,1,0,1,1,0,0,
                  0,0,1,0,1,0,0,0,
                  0,0,0,0,1,0,0,0,
                  0,0,0,0,1,0,0,0};         
    
    // execute A-star
    std::pair<int,int> start(1,1);
    std::pair<int,int> goal(5,6);
                  
    find_path(path, wall, 8,8, start, goal );
    
    // print path
    for(std::vector< std::pair<int, int> >::const_reverse_iterator i = path.rbegin(); i!= path.rend(); ++i)
    {
      std::cout << i->first << ' ' << i->second << '\n';
    }
    
    // draw map
    char draw[8*8];         
    
    for(int i = 0;i<8*8;++i)
    {
      draw[i] = wall[i] ? '#' : '.';
    }
    
    for(std::vector< std::pair<int, int> >::const_reverse_iterator i = path.rbegin(); i!= path.rend(); ++i)
    {
      draw[i->first + i->second * 8 ] = '*';
    }
    
    draw[start.first + start.second * 8 ] = 'S';
    draw[goal.first + goal.second * 8 ] = 'G';
    
    for(int i = 0;i<8*8;++i)
    {
      std::cout << draw[i];
      if((i&0x07)==0x07) std::cout << std::endl;
    }
    
}