//////////////////////////////////////////////////////////////////////
// Amit's Path-finding (A*) code.
//
// Copyright (C) 1999 Amit J. Patel
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  Amit J. Patel makes no
// representations about the suitability of this software for any
// purpose.  It is provided "as is" without express or implied warranty.
//
//
// This code is not self-contained.  It compiles in the context of
// my game (SimBlob) and will need modification to work in another
// program.  I am providing it as a base to work from, and not as
// a finished library.
//
// The main items of interest in my code are:
// 
// 1. I'm using a hexagonal grid instead of a square grid.  Since A*
//    on a square grid works better with the "Manhattan" distance than with
//    straight-line distance, I wrote a "Manhattan" distance on a hexagonal
//    grid.  I also added in a penalty for paths that are not straight
//    lines.  This makes lines turn out straight in the simplest case (no
//    obstacles) without using a straight-line distance function (which can
//    make the path finder much slower).
//
//    To see the distance function, search for UnitMovement and look at
//    its 'dist' function.
//
// 2. The cost function is adjustable at run-time, allowing for a
//    sort of "slider" that varies from "Fast Path Finder" to "Good Path
//    Quality".  (My notes on A* have some ways in which you can use this.)
//
// 3. I'm using a data structure called a "heap" instead of an array
//    or linked list for my OPEN set.  Using lists or arrays, an
//    insert/delete combination takes time O(N); with heaps, an
//    insert/delete combination takes time O(log N).  When N (the number of
//    elements in OPEN) is large, this can be a big win.  However, heaps
//    by themselves are not good for one particular operation in A*.
//    The code here avoids that operation most of the time by using
//    a "Marking" array.  For more information about how this helps
//    avoid a potentially expensive operation, see my Implementation
//    Nodes in my notes on A*.
//
//  Thanks to Rob Rodrigues dos santos Jr for pointing out some
//  editing bugs in the version of the code I put up on the web.
//////////////////////////////////////////////////////////////////////

#include "Path.h"

// The mark array marks directions on the map.  The direction points
// to the spot that is the previous spot along the path.  By starting
// at the end, we can trace our way back to the start, and have a path.
// It also stores 'f' values for each space on the map.  These are used
// to determine whether something is in OPEN or not.  It stores 'g'
// values to determine whether costs need to be propagated down.
struct Marking
{
    HexDirection direction:4;    // !DirNone means OPEN || CLOSED
    int f:14;                    // >= 0 means OPEN
    int g:14;                    // >= 0 means OPEN || CLOSED
    Marking(): direction(DirNone), f(-1), g(-1) {}
};
static MapArray<Marking>& mark = *(new MapArray<Marking>(Marking()));

// Path_div is used to modify the heuristic.  The lower the number,
// the higher the heuristic value.  This gives us worse paths, but
// it finds them faster.  This is a variable instead of a constant
// so that I can adjust this dynamically, depending on how much CPU
// time I have.  The more CPU time there is, the better paths I should
// search for.
int path_div = 6;

struct Node
{
    HexCoord h;        // location on the map, in hex coordinates
    int gval;        // g in A* represents how far we've already gone
    int hval;        // h in A* represents an estimate of how far is left

    Node(): h(0,0), gval(0), hval(0) {}
    ~Node() {}
};

bool operator < ( const Node& a, const Node& b )
{
    // To compare two nodes, we compare the `f' value, which is the
    // sum of the g and h values.
    return (a.gval+a.hval) < (b.gval+b.hval);
}

bool operator == ( const Node& a, const Node& b )
{
    // Two nodes are equal if their components are equal
    return (a.h == b.h) && (a.gval == b.gval) && (a.hval == b.hval);
}

inline HexDirection ReverseDirection( HexDirection d )
{
    // With hexagons, I'm numbering the directions 0 = N, 1 = NE,
    // and so on (clockwise).  To flip the direction, I can just
    // add 3, mod 6.
    return HexDirection( ( 3+int(d) ) % 6 );
}

// greater<Node> is an STL thing to create a 'comparison' object out of
// the greater-than operator, and call it comp.
typedef vector<Node> Container;
greater<Node> comp;

// I'm using a priority queue implemented as a heap.  STL has some nice
// heap manipulation functions.  (Look at the source to `priority_queue'
// for details.)  I didn't use priority_queue because later on, I need
// to traverse the entire data structure to update certain elements; the
// abstraction layer on priority_queue wouldn't let me do that.
inline void get_first( Container& v, Node& n )
{
    n = v.front();
    pop_heap( v.begin(), v.end(), comp );
    v.pop_back();
}

// Here's the class that implements A*.  I take a map, two points
// (A and B), and then output the path in the `path' vector, when
// find_path is called.
template <class Heuristic>
struct AStar
{
    PathStats stats;
    Heuristic& heuristic;
    // Remember which nodes are in the OPEN set
    Container open;
    // Remember which nodes we visited, so that we can clear the mark array
    // at the end.  This is the 'CLOSED' set plus the 'OPEN' set.
    Container visited;
    Map& map;
    HexCoord A, B;
    
    AStar( Heuristic& h, Map& m, HexCoord a, HexCoord b )
        : heuristic(h), map(m), A(a), B(b) {}
    ~AStar();

    // Main function:
    void find_path( vector<HexCoord>& path );

    // Helpers:
    void propagate_down( Node H );
    Container::iterator find_in_open( HexCoord h );
    inline bool in_open( const HexCoord& h )
    {
        return mark.data[h.m][h.n].f != -1;
    }
};

template<class Heuristic>
AStar<Heuristic>::~AStar()
{
    // Erase the mark array, for all items in open or visited
    for( Container::iterator o = open.begin(); o != open.end(); ++o )
    {
        HexCoord h = (*o).h;
        mark.data[h.m][h.n].direction = DirNone;
        mark.data[h.m][h.n].f = -1;
        mark.data[h.m][h.n].g = -1;
    }
    for( Container::iterator v = visited.begin(); v != visited.end(); ++v )
    {
        HexCoord h = (*v).h;
        mark.data[h.m][h.n].direction = DirNone;
        mark.data[h.m][h.n].g = -1;
        assert( !in_open( h ) );
    }
}

template <class Heuristic>
Container::iterator AStar<Heuristic>::find_in_open( HexCoord hn )
{
    // Only search for this node if we know it's in the OPEN set
    if( Map::valid(hn) && in_open(hn) ) 
    {
        for( Container::iterator i = open.begin(); i != open.end(); ++i )
        {
            stats.nodes_searched++;
            if( (*i).h == hn )
                return i;
        }
    }
    return open.end();
}

// This is the 'propagate down' stage of the algorithm, which I'm not
// sure I did right.
template <class Heuristic>
void AStar<Heuristic>::propagate_down( Node H )
{
    // Keep track of the nodes that we still have to consider
    Container examine;
    examine.push_back(H);
    while( !examine.empty() )
    {
        // Remove one node from the list
        Node N = examine.back();
        examine.pop_back();

        // Examine its neighbors
        for( int dir = 0; dir < 6; ++dir )
        {
            HexDirection d = HexDirection(dir);
            HexCoord hn = Neighbor( N.h, d );
            if( in_open(hn) )
            {
                // This node is in OPEN                
                int new_g = N.gval + heuristic.kost( map, N.h, d, hn );

                // Compare this `g' to the stored `g' in the array
                if( new_g < mark.data[hn.m][hn.n].g )
                {
                    Container::iterator i = find_in_open( hn );
                    assert( i != open.end() );
                    assert( mark.data[hn.m][hn.n].g == (*i).gval );
                    
                    // Push this thing UP in the heap (only up allowed!)
                    (*i).gval = new_g;
                    push_heap( open.begin(), i+1, comp );

                    // Set its direction to the parent node
                    mark.data[hn.m][hn.n].g = new_g;
                    mark.data[hn.m][hn.n].f = new_g + (*i).hval;
                    mark.data[hn.m][hn.n].direction = ReverseDirection(d);
                        
                    // Now reset its parent 
                    examine.push_back( (*i) );
                }
                else
                {
                    // The new node is no better, so stop here
                }
            }
            else
            {
                // Either it's in closed, or it's not visited yet
            }
        }
    }
}

template <class Heuristic>
void AStar<Heuristic>::find_path( vector<HexCoord>& path )
{
    Node N;
    {
        // insert the original node
        N.h = A;
        N.gval = 0;
        N.hval = heuristic.dist(map,A,B);
        open.push_back(N);
        mark.data[A.m][A.n].f = N.gval+N.hval;
        mark.data[A.m][A.n].g = N.gval;
        stats.nodes_added++;
    }

    // * Things in OPEN are in the open container (which is a heap),
    //   and also their mark[...].f value is nonnegative.
    // * Things in CLOSED are in the visited container (which is unordered),
    //   and also their mark[...].direction value is not DirNone.

    // While there are still nodes to visit, visit them!
    while( !open.empty() )
    {
        get_first( open, N );
        mark.data[N.h.m][N.h.n].f = -1;
        visited.push_back( N );
        stats.nodes_removed++;
        
        // If we're at the goal, then exit
        if( N.h == B )
            break;

        // If we've looked too long, then exit
        if( stats.nodes_removed >= heuristic.abort_path )
        {
            // Select a good element of OPEN
            for( Container::iterator i = open.begin(); i != open.end(); ++i )
            {
                if( (*i).hval*2 + (*i).gval < N.hval*2 + N.gval )
                    N = *i;
            }
            
            B = N.h;
            break;
        }

        // Every other column gets a different order of searching dirs
        // (Alternatively, you could pick one at random).  I don't want
        // to be too biased by my choice of order in which I look at the
        // neighboring grid spots.
        int directions1[6] = {0,1,2,3,4,5};
        int directions2[6] = {5,4,3,2,1,0};
        int *directions;
        if( (N.h.m+N.h.n) % 2 == 0 )
            directions = directions1;
        else
            directions = directions2;

        // Look at your neighbors.
        for( int dci = 0; dci < 6; ++dci )
        {
            HexDirection d = HexDirection(directions[dci]);
            HexCoord hn = Neighbor( N.h, d );
            // If it's off the end of the map, then don't keep scanning
            if( !map.valid(hn) )
                continue;

            int k = heuristic.kost(map, N.h, d, hn);
            Node N2;
            N2.h = hn;
            N2.gval = N.gval + k;
            N2.hval = heuristic.dist( map, hn, B );
            // If this spot (hn) hasn't been visited, its mark is DirNone
            if( mark.data[hn.m][hn.n].direction == DirNone )
            {
                // The space is not marked
                mark.data[hn.m][hn.n].direction = ReverseDirection(d);
                mark.data[hn.m][hn.n].f = N2.gval+N2.hval;
                mark.data[hn.m][hn.n].g = N2.gval;
                open.push_back( N2 );
                push_heap( open.begin(), open.end(), comp );
                stats.nodes_added++;
            }
            else
            {                
                // We know it's in OPEN or CLOSED...
                if( in_open(hn) )
                {
                    // It's in OPEN, so figure out whether g is better
                    if( N2.gval < mark.data[hn.m][hn.n].g )
                    {
                        // Search for hn in open
                        Container::iterator find1 = find_in_open( hn );
                        assert( find1 != open.end() );
                        
                        // Replace *find1's gval with N2.gval in the list&map
                        mark.data[hn.m][hn.n].direction = ReverseDirection(d);
                        mark.data[hn.m][hn.n].g = N2.gval;
                        mark.data[hn.m][hn.n].f = N2.gval+N2.hval;
                        (*find1).gval = N2.gval;
                        // This is allowed but it's not obvious why:
                        push_heap( open.begin(), find1+1, comp );
                        // (ask Amit if you're curious about it)

                        // This next step is not needed for most games
                        propagate_down( *find1 );
                    }
                }
            }
        }
    }

    if( N.h == B && N.gval < MAXIMUM_PATH_LENGTH )
    {
        stats.path_cost = N.gval;
        // We have found a path, so let's copy it into `path'
        HexCoord h = B;
        while( h != A )
        {
            HexDirection dir = mark.data[h.m][h.n].direction;
            path.push_back( h );
            h = Neighbor( h, dir );
            stats.path_length++;
        }
        path.push_back( A );
        // path now contains the hexes in which the unit must travel ..
        // backwards (like a stack)
    }
    else
    {
        // No path
    }

    stats.nodes_left = open.size();
    stats.nodes_visited = visited.size();
}

////////////////////////////////////////////////////////////////////////
// Specific instantiations of A* for different purposes

// UnitMovement is for moving units (soldiers, builders, firefighters)
struct UnitMovement
{
    HexCoord source;
    Unit* unit;
    int abort_path;
    
    inline static int dist( const HexCoord& a, const HexCoord& b )
    {
        // The **Manhattan** distance is what should be used in A*'s heuristic
        // distance estimate, *not* the straight-line distance.  This is because
        // A* wants to know the estimated distance for its paths, which involve
        // steps along the grid.  (Of course, if you assign 1.4 to the cost of
        // a diagonal, then you should use a distance function close to the
        // real distance.)

        // Here I compute a ``Manhattan'' distance for hexagons.  Nifty, eh?
        int a1 = 2*a.m;
        int a2 =  2*a.n+a.m%2 - a.m;
        int a3 = -2*a.n-a.m%2 - a.m; // == -a1-a2
        int b1 = 2*b.m;
        int b2 =  2*b.n+b.m%2 - b.m;
        int b3 = -2*b.n-b.m%2 - b.m; // == -b1-b2

        // One step on the map is 10 in this function
        return 5*max(abs(a1-b1), max(abs(a2-b2), abs(a3-b3)));
    }

    inline int dist( Map& m, const HexCoord& a, const HexCoord& b )
    {
        double dx1 = a.x() - b.x();
        double dy1 = a.y() - b.y();
        double dx2 = source.x() - b.x();
        double dy2 = source.y() - b.y();
        double cross = dx1*dy2-dx2*dy1;
        if( cross < 0 ) cross = -cross;

        return dist( a, b ) + int(cross/20000);
    }
    
    inline int kost( Map& m, const HexCoord& a, 
                     HexDirection d, const HexCoord& b, int pd = -1 )
    {
        // This is the cost of moving one step.  To get completely accurate
        // paths, this must be greater than or equal to the change in the
        // distance function when you take a step.

        if( pd == -1 ) pd = path_div;
        
        // Check for neighboring moving obstacles
        int occ = m.occupied_[b];
        if( ( occ != -1 && m.units[occ] != unit ) &&
            ( !m.units[occ]->moving() || ( source == a && d != DirNone ) ) )
                return MAXIMUM_PATH_LENGTH;

        // Roads are faster (twice as fast), and cancel altitude effects
        Terrain t1 = m.terrain(a);
        Terrain t2 = m.terrain(b);
        //        int rd = int((t2==Road||t2==Bridge)&&(t1==Road||t2==Bridge));
        // It'd be better theoretically for roads to work only when both
        // hexes are roads, BUT the path finder works faster when
        // it works just when the destination is a road, because it can
        // just step onto a road and know it's going somewhere, as opposed
        // to having to step on the road AND take another step.
        int rd = int(t2==Road || t2==Bridge);
        int rdv = ( 5 - 10 * rd ) * (pd - 3) / 5;
        // Slow everyone down on gates, canals, or walls
        if( t2 == Gate || t2 == Canal )
            rdv += 50;
        if( t2 == Wall )
            rdv += 150;
        // Slow down everyone on water, unless it's on a bridge
        if( t2 != Bridge && m.water(b) > 0 )
            rdv += 30;
        // If there's no road, I take additional items into account
        if( !rd )
        {
            // One thing we can do is penalize for getting OFF a road
            if( t1==Road || t1==Bridge )
                rdv += 15;
            // I take the difference in altitude and use that as a cost,
            // rounded down, which means that small differences cost 0
            int da = (m.altitude(b)-m.altitude(a))/ALTITUDE_SCALE;
            if( da > 0 )
                rdv += da * (pd-3);
        }
        return 10 + rdv;
    }
};

// Some useful functions are exported to be used without the pathfinder

int hex_distance( HexCoord a, HexCoord b )
{
    return UnitMovement::dist( a, b );
}

int movement_cost( Map& m, HexCoord a, HexCoord b, Unit* unit )
{
    UnitMovement um;
    um.unit = unit;
    return um.kost( m, a, DirNone, b, 8 );
}

// BuildingMovement is for drawing straight lines (!)
struct BuildingMovement
{
    HexCoord source;
    int abort_path;
    
    inline int dist( Map& m, const HexCoord& a, const HexCoord& b )
    {
        double dx1 = a.x() - b.x();
        double dy1 = a.y() - b.y();
        double dd1 = dx1*dx1+dy1*dy1;
        // The cross product will be high if two vectors are not colinear
        // so we can calculate the cross product of [current->goal] and
        // [source->goal] to see if we're staying along the [source->goal]
        // vector.  This will help keep us in a straight line.
        double dx2 = source.x() - b.x();
        double dy2 = source.y() - b.y();
        double cross = dx1*dy2-dx2*dy1;
        if( cross < 0 ) cross = -cross;
        return int( dd1 + cross );
    }

    inline int kost( Map& m, const HexCoord& a, 
                     HexDirection d, const HexCoord& b )
    {
        return 0;
    }
};

// Flat Canal movement tries to find a path for a canal that is not too steep
struct FlatCanalPath: public UnitMovement
{
    int kost( Map& m, const HexCoord& a, 
                     HexDirection d, const HexCoord& b )
    {
        // Try to minimize the slope
        int a0 = m.altitude(a);
        int bda = 0;
        for( int dir = 0; dir < 6; ++dir )
        {            
            int da = a0-m.altitude( Neighbor(a,HexDirection(dir)) );
            if( da > bda ) bda = da;
        }
        return 1 + 100*bda*bda;
    }
};

//////////////////////////////////////////////////////////////////////
// These functions call AStar with the proper heuristic object

PathStats FindUnitPath( Map& map, HexCoord A, HexCoord B, 
                        vector<HexCoord>& path, Unit* unit, int cutoff )
{
    UnitMovement um;    
    um.source = A;
    um.unit = unit;
    um.abort_path = cutoff * hex_distance(A,B) / 10;

    AStar<UnitMovement> finder( um, map, A, B );

    // If the goal node is unreachable, don't even try
    if( um.kost( map, A, DirNone, B ) == MAXIMUM_PATH_LENGTH )
    {
        // Check to see if a neighbor is reachable.  This is specific
        // to SimBlob and not something for A* -- I want to find a path
        // to a neighbor if the original goal was unreachable (perhaps it
        // is occupied or unpassable).
        int cost = MAXIMUM_PATH_LENGTH;
        HexCoord Bnew = B;
        for( int d = 0; d < 6; ++d )
        {
            HexCoord hn = Neighbor( B, HexDirection(d) );
            int c = um.kost( map, A, DirNone, hn );
            if( c < cost )
            {
                // This one is closer, hopefully
                Bnew = B;
                cost = c;
            }
        }
        // New goal
        B = Bnew;

        if( cost == MAXIMUM_PATH_LENGTH )
        {
            // No neighbor was good
            return finder.stats;
        }
    }

    finder.find_path( path );
    return finder.stats;
}

PathStats FindBuildPath( Map& map, HexCoord A, HexCoord B,
                         vector<HexCoord>& path )
{
    BuildingMovement bm;
    bm.source = A;

    AStar<BuildingMovement> finder( bm, map, A, B );
    finder.find_path( path );
    return finder.stats;
}

PathStats FindCanalPath( Map& map, HexCoord A, HexCoord B,
                         vector<HexCoord>& path )
{
    FlatCanalPath fcp;
    fcp.source = A;

    AStar<FlatCanalPath> finder( fcp, map, A, B );
    finder.find_path( path );
    return finder.stats;
}