adaptagrams/libavoid/makepath.cpp

/*
 * vim: ts=4 sw=4 et tw=0 wm=0
 *
 * libavoid - Fast, Incremental, Object-avoiding Line Router
 *
 * Copyright (C) 2004-2014  Monash University
 *
 * 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.
 * See the file LICENSE.LGPL distributed with the library.
 *
 * Licensees holding a valid commercial license may use this file in
 * accordance with the commercial license agreement provided with the
 * library.
 *
 * 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.
 *
 * Author(s):  Michael Wybrow
*/

// For M_PI.
// This should be first include for MSVC.
#define _USE_MATH_DEFINES
#include <cmath>

#include <algorithm>
#include <vector>
#include <climits>
#include <cfloat>

#include "libavoid/makepath.h"
#include "libavoid/vertices.h"
#include "libavoid/geometry.h"
#include "libavoid/connector.h"
#include "libavoid/viscluster.h"
#include "libavoid/graph.h"
#include "libavoid/router.h"
#include "libavoid/debug.h"
#include "libavoid/assertions.h"
#include "libavoid/debughandler.h"

//#define ESTIMATED_COST_DEBUG

namespace Avoid {

class ANode
{
    public:
        VertInf* inf;
        double g;        // Gone
        double h;        // Heuristic
        double f;        // Formula f = g + h

        ANode *prevNode; // VertInf for the previous ANode.
        int timeStamp;   // Time-stamp used to determine exploration order of
                         // seemingly equal paths during orthogonal routing.

        ANode(VertInf *vinf, int time)
            : inf(vinf),
              g(0),
              h(0),
              f(0),
              prevNode(nullptr),
              timeStamp(time)
        {
        }
        ANode()
            : inf(nullptr),
              g(0),
              h(0),
              f(0),
              prevNode(nullptr),
              timeStamp(-1)
        {
        }
};

class AStarPathPrivate
{
    public:
        AStarPathPrivate()
            : m_available_nodes(),
              m_available_array_size(0),
              m_available_array_index(0),
              m_available_node_index(0)
        {
        }
        ~AStarPathPrivate()
        {
            // Free memory
            for (size_t i = 0; i < m_available_nodes.size(); ++i)
            {
                delete[] m_available_nodes[i];
            }
        }
        // Returns a pointer to an ANode for aStar search, but allocates
        // these in blocks
        ANode *newANode(const ANode& node, const bool addToPending = true)
        {
            const size_t blockSize = 5000;
            if ((m_available_array_index + 1 > m_available_array_size) ||
                    (m_available_node_index >= blockSize))
            {
                m_available_nodes.push_back(new ANode[blockSize]);
                ++m_available_array_size;
                m_available_node_index = 0;
                m_available_array_index = m_available_array_size - 1;
            }

            ANode *nodes = m_available_nodes[m_available_array_index];
            ANode *newNode = &(nodes[m_available_node_index++]);
            *newNode = node;
            if (addToPending)
            {
                node.inf->aStarPendingNodes.push_back(newNode);
            }
            return newNode;
        }
        void search(ConnRef *lineRef, VertInf *src, VertInf *tar,
                VertInf *start);

    private:
        void determineEndPointLocation(double dist, VertInf *start,
                VertInf *target, VertInf *other, int level);
        double estimatedCost(ConnRef *lineRef, const Point *last,
                const Point& curr) const;

        std::vector<ANode *> m_available_nodes;
        size_t m_available_array_size;
        size_t m_available_array_index;
        size_t m_available_node_index;

        // For determining estimated cost target.
        std::vector<VertInf *> m_cost_targets;
        std::vector<unsigned int> m_cost_targets_directions;
        std::vector<double> m_cost_targets_displacements;
};


// This returns the opposite result (>) so that when used with stl::make_heap,
// the head node of the heap will be the smallest value, rather than the
// largest.  This saves us from having to sort the heap (and then reorder
// it back into a heap) when getting the next node to examine.  This way we
// get better complexity -- logarithmic pushes and pops to the heap.
//
class ANodeCmp
{
    public:
    ANodeCmp()
    {
    }
bool operator()(const ANode *a, const ANode *b)
{
    // We need to use an epsilon here since otherwise the multiple addition
    // of floating point numbers that makes up the 'f' values cause a problem
    // with routings occasionally being non-deterministic.
    if (fabs(a->f - b->f) > 0.0000001)
    {
        return a->f > b->f;
    }
    if (a->timeStamp != b->timeStamp)
    {
        // Tiebreaker, if two paths have equal cost, then choose the one with
        // the highest timeStamp.  This corresponds to the furthest point
        // explored along the straight-line path.  When exploring we give the
        // directions the following timeStamps; left:1, right:2 and forward:3,
        // then we always try to explore forward first.
        return a->timeStamp < b->timeStamp;
    }
    return false;
}
};


static double Dot(const Point& l, const Point& r)
{
    return (l.x * r.x) + (l.y * r.y);
}

static double CrossLength(const Point& l, const Point& r)
{
    return (l.x * r.y) - (l.y * r.x);
}


// Return the angle between the two line segments made by the
// points p1--p2 and p2--p3.  Return value is in radians.
//
static double angleBetween(const Point& p1, const Point& p2, const Point& p3)
{
    if ((p1.x == p2.x && p1.y == p2.y) || (p2.x == p3.x && p2.y == p3.y))
    {
        // If two of the points are the same, then we can't say anything
        // about the angle between.  Treat them as being collinear.
        return M_PI;
    }

    Point v1(p1.x - p2.x, p1.y - p2.y);
    Point v2(p3.x - p2.x, p3.y - p2.y);

    return fabs(atan2(CrossLength(v1, v2), Dot(v1, v2)));
}


// Construct a temporary Polygon path given several VertInf's for a connector.
//
static void constructPolygonPath(Polygon& connRoute, VertInf *inf2,
        VertInf *inf3, ANode *inf1Node)
{
    // Don't include colinear points.
    bool simplified = true;

    int routeSize = 2;
    for (ANode *curr = inf1Node; curr != nullptr; curr = curr->prevNode)
    {
        routeSize += 1;
    }
    connRoute.ps.resize(routeSize);
    int arraySize = routeSize;
    connRoute.ps[routeSize - 1] = inf3->point;
    connRoute.ps[routeSize - 2] = inf2->point;
    routeSize -= 3;
    for (ANode *curr = inf1Node; curr != nullptr; curr = curr->prevNode)
    {
        // For connection pins, we stop and don't include the fake shape
        // center as part of this path.
        bool isConnectionPin = curr->inf->id.isConnectionPin();

        if (!simplified)
        {
            // If this is non-simplified, we don't need to do anything
            // clever and can simply add the new point.
            connRoute.ps[routeSize] = curr->inf->point;
            routeSize -= 1;

            if (isConnectionPin)
            {
                // Stop at the connection pin.
                break;
            }
            continue;
        }

        if ((curr == inf1Node) ||
                vecDir(curr->inf->point, connRoute.ps[routeSize + 1],
                    connRoute.ps[routeSize + 2]) != 0)
        {
            // Add new point if this is the earlier than the last segment
            // and it is not colinear with the other points.
            // Note, you can't collapse the 'last' segment with previous
            // segments, or if this just intersects another line you risk
            // penalising it once for each collapsed line segment.
            connRoute.ps[routeSize] = curr->inf->point;
            routeSize -= 1;
        }
        else
        {
            // The last point is inline with this one, so update it.
            connRoute.ps[routeSize + 1] = curr->inf->point;
        }

        if (isConnectionPin)
        {
            // Stop at the connection pin.
            break;
        }
    }

    // If the vector is not filled, move entries to the beginning and
    // remove the unused end of the vector.
    int diff = routeSize + 1;
    COLA_ASSERT(simplified || (diff == 0));
    if (diff > 0)
    {
        for (int i = diff; i < arraySize; ++i)
        {
            connRoute.ps[i - diff] = connRoute.ps[i];
        }
        connRoute.ps.resize(connRoute.size() - diff);
    }
}

// Used to get an indication of if a diffence is positive (1),
// negative (-1) or no different (0).
static inline int dimDirection(double difference)
{
    if (difference > 0)
    {
        return 1;
    }
    else if (difference < 0)
    {
        return -1;
    }
    return 0;
}

// Given the two points for a new segment of a path (inf2 & inf3)
// as well as the distance between these points (dist), as well as
// possibly the previous point (inf1) [from inf1--inf2], return a
// cost associated with this route.
//
static double cost(ConnRef *lineRef, const double dist, VertInf *inf2,
        VertInf *inf3, ANode *inf1Node)
{
    bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);
    VertInf *inf1 = (inf1Node) ? inf1Node->inf : nullptr;
    double result = dist;
    Polygon connRoute;

    Router *router = inf2->_router;
    if (inf1 != nullptr)
    {
        const double angle_penalty = router->routingParameter(anglePenalty);
        const double segmt_penalty = router->routingParameter(segmentPenalty);

        // This is not the first segment, so there is a bend
        // between it and the last one in the existing path.
        if ((angle_penalty > 0) || (segmt_penalty > 0))
        {
            Point p1 = inf1->point;
            Point p2 = inf2->point;
            Point p3 = inf3->point;

            double rad = M_PI - angleBetween(p1, p2, p3);

            if ((rad > 0) && !isOrthogonal)
            {
                // Make `xval' between 0--10 then take its log so small
                // angles are not penalised as much as large ones.
                //
                double xval = rad * 10 / M_PI;
                double yval = xval * log10(xval + 1) / 10.5;
                result += (angle_penalty * yval);
                //db_printf("deg from straight: %g\tpenalty: %g\n",
                //        rad * 180 / M_PI, (angle_penalty * yval));
            }

            if (rad == M_PI)
            {
                // Needs to double back
                result += (2 * segmt_penalty);
            }
            else if (rad > 0)
            {
                // Only penalise as an extra segment if the two
                // segments are not collinear.
                result += segmt_penalty;
            }
        }
    }

    const double cluster_crossing_penalty =
            router->routingParameter(clusterCrossingPenalty);
    // XXX: Clustered routing doesn't yet work with orthogonal connectors.
    if (router->ClusteredRouting && !router->clusterRefs.empty() &&
            (cluster_crossing_penalty > 0))
    {
        if (connRoute.empty())
        {
            constructPolygonPath(connRoute, inf2, inf3, inf1Node);
        }
        // There are clusters so do cluster routing.
        for (ClusterRefList::const_iterator cl = router->clusterRefs.begin();
                cl != router->clusterRefs.end(); ++cl)
        {
            Polygon cBoundary = (isOrthogonal) ?
                    (*cl)->rectangularPolygon() : (*cl)->polygon();
            if (cBoundary.size() <= 2)
            {
                continue;
            }
            COLA_ASSERT(cBoundary.ps[0] != cBoundary.ps[cBoundary.size() - 1]);
            for (size_t j = 0; j < cBoundary.size(); ++j)
            {
                // Non-orthogonal cluster boundary points should correspond to
                // shape vertices and hence already be in the list of vertices.
                COLA_ASSERT(isOrthogonal ||
                        router->vertices.getVertexByPos(cBoundary.at(j)));
            }

            bool isConn = false;
            Polygon dynamic_conn_route(connRoute);
            const bool finalSegment = (inf3 == lineRef->dst());
            ConnectorCrossings cross(cBoundary, isConn, dynamic_conn_route);
            cross.checkForBranchingSegments = true;
            cross.countForSegment(connRoute.size() - 1, finalSegment);

            result += (cross.crossingCount * cluster_crossing_penalty);
        }
    }

    // This penalty penalises route segments that head in a direction opposite
    // of the direction(s) toward the target point.
    const double reversePenalty = router->routingParameter(
            reverseDirectionPenalty);
    if (reversePenalty)
    {
        // X and Y direction of destination from source point.
        const Point& srcPoint = lineRef->src()->point;
        const Point& dstPoint = lineRef->dst()->point;
        int xDir = dimDirection(dstPoint.x - srcPoint.x);
        int yDir = dimDirection(dstPoint.y - srcPoint.y);

        bool doesReverse = false;

        if ((xDir != 0) &&
                (-xDir == dimDirection(inf3->point.x - inf2->point.x)))
        {
            // Connector has an X component and the segment heads in the
            // opposite direction.
            doesReverse |= true;
        }

        if ((yDir != 0) &&
                (-yDir == dimDirection(inf3->point.y - inf2->point.y)))
        {
            // Connector has an Y component and the segment heads in the
            // opposite direction.
            doesReverse |= true;
        }

        if (doesReverse)
        {
            result += reversePenalty;
        }
    }

    if (!router->isInCrossingPenaltyReroutingStage())
    {
        // Return here if we are not in the post-processing stage
        return result;
    }

    const double crossing_penalty = router->routingParameter(crossingPenalty);
    const double shared_path_penalty =
            router->routingParameter(fixedSharedPathPenalty);
    if ((shared_path_penalty > 0) || (crossing_penalty > 0))
    {
        if (connRoute.empty())
        {
            constructPolygonPath(connRoute, inf2, inf3, inf1Node);
        }
        ConnRefList::const_iterator curr, finish = router->connRefs.end();
        for (curr = router->connRefs.begin(); curr != finish; ++curr)
        {
            ConnRef *connRef = *curr;

            if (connRef->id() == lineRef->id())
            {
                continue;
            }
            const Avoid::PolyLine& route2 = connRef->displayRoute();

            bool isConn = true;
            Polygon dynamic_route2(route2);
            Polygon dynamic_conn_route(connRoute);
            const bool finalSegment = (inf3->point == lineRef->dst()->point);
            ConnectorCrossings cross(dynamic_route2, isConn,
                    dynamic_conn_route, connRef, lineRef);
            cross.checkForBranchingSegments = true;
            cross.countForSegment(connRoute.size() - 1, finalSegment);

            if ((cross.crossingFlags & CROSSING_SHARES_PATH) &&
                    (cross.crossingFlags & CROSSING_SHARES_FIXED_SEGMENT) &&
                    (router->routingOption(
                            penaliseOrthogonalSharedPathsAtConnEnds) ||
                     !(cross.crossingFlags & CROSSING_SHARES_PATH_AT_END)))
            {
                // Penalise unnecessary shared paths in the middle of
                // connectors.
                result += shared_path_penalty;
            }
            result += (cross.crossingCount * crossing_penalty);
        }
    }

    return result;
}

// Directions for estimated orthgonal cost, as bitflags.
static const unsigned int CostDirectionN = 1;
static const unsigned int CostDirectionE = 2;
static const unsigned int CostDirectionS = 4;
static const unsigned int CostDirectionW = 8;

#ifdef ESTIMATED_COST_DEBUG
static void printDirections(FILE *fp, unsigned int directions)
{
    if (directions & CostDirectionN)
    {
        fprintf(fp, "N ");
    }
    if (directions & CostDirectionE)
    {
        fprintf(fp, "E ");
    }
    if (directions & CostDirectionS)
    {
        fprintf(fp, "S ");
    }
    if (directions & CostDirectionW)
    {
        fprintf(fp, "W ");
    }
}
#endif

// Returns the number of directions for the argument.
static unsigned int orthogonalDirectionsCount(const unsigned int directions)
{
    unsigned int count = 0;
    if (directions & CostDirectionN)
    {
        ++count;
    }
    if (directions & CostDirectionE)
    {
        ++count;
    }
    if (directions & CostDirectionS)
    {
        ++count;
    }
    if (directions & CostDirectionW)
    {
        ++count;
    }
    return count;
}

// Returns the directions of point b from point a.
static unsigned int orthogonalDirection(const Point &a, const Point &b)
{
    unsigned int result = 0;

    if (b.y > a.y)
    {
        result |= CostDirectionS;
    }
    else if (b.y < a.y)
    {
        result |= CostDirectionN;
    }

    if (b.x > a.x)
    {
        result |= CostDirectionE;
    }
    else if (b.x < a.x)
    {
        result |= CostDirectionW;
    }

    return result;
}

// Returns the direction to the right of the given direction.
static unsigned int dirRight(unsigned int direction)
{
    if (direction == CostDirectionN)
    {
        return CostDirectionE;
    }
    else if (direction == CostDirectionE)
    {
        return CostDirectionS;
    }
    else if (direction == CostDirectionS)
    {
        return CostDirectionW;
    }
    else if (direction == CostDirectionW)
    {
        return CostDirectionN;
    }

    // Should not be possible to reach here.
    COLA_ASSERT(false);
    return direction;
}

// Returns the direction to the left of the given direction.
static unsigned int dirLeft(unsigned int direction)
{
    if (direction == CostDirectionN)
    {
        return CostDirectionW;
    }
    else if (direction == CostDirectionE)
    {
        return CostDirectionN;
    }
    else if (direction == CostDirectionS)
    {
        return CostDirectionE;
    }
    else if (direction == CostDirectionW)
    {
        return CostDirectionS;
    }

    // Should not be possible to reach here.
    COLA_ASSERT(false);
    return direction;
}

// Returns the reverse direction to the given direction.
static unsigned int dirReverse(unsigned int direction)
{
    if (direction == CostDirectionN)
    {
        return CostDirectionS;
    }
    else if (direction == CostDirectionE)
    {
        return CostDirectionW;
    }
    else if (direction == CostDirectionS)
    {
        return CostDirectionN;
    }
    else if (direction == CostDirectionW)
    {
        return CostDirectionE;
    }

    // Should not be possible to reach here.
    COLA_ASSERT(false);
    return direction;
}

// Given Point curr with a direction of currDir, returns the nimimum number
// of bends to reach Point dest with the entry direction of destDir
//
// This is used for estimating the bend penalty cost to the target point
// from the current point of the search. The geometry was described in the
// "Orthogonal Connector Routing" paper, although the version described
// there is incorrect.
//
int bends(const Point& curr, unsigned int currDir, const Point& dest,
        unsigned int destDir)
{
        // Bend counts from 'o' to 'D' should be:
        //
        //                1            1            3
        //                v            v            v
        //            2 > o < 2    2 > o < 2    4 > o < 2
        //                ^            ^            ^
        //                3            3            3
        //
        //   0 > o < 4                 D-->             4 > o < 4
        //       ^                                          ^
        //       1                                          3
        //
    COLA_ASSERT(currDir != 0);
    unsigned int currToDestDir = orthogonalDirection(curr, dest);
    unsigned int reverseDestDir = dirReverse(destDir);
    bool currDirPerpendicularToDestDir =
            (currDir == dirLeft(destDir)) || (currDir == dirRight(destDir));

    if ((currDir == destDir) &&
             (currToDestDir == currDir))
    {
        //
        //   0 > o                     D-->
        //
        return 0;
    }
    else if (currDirPerpendicularToDestDir &&
             (currToDestDir == (destDir | currDir)))
    {
        //
        //                1
        //                v
        //                o
        //
        //
        //                             D-->
        //
        return 1;
    }
    else if (currDirPerpendicularToDestDir &&
             (currToDestDir == currDir))
    {
        //
        //                             1
        //                             v
        //                             o
        //
        //
        //                             D-->
        //
        return 1;
    }
    else if (currDirPerpendicularToDestDir &&
             (currToDestDir == destDir))
    {
        //
        //       o                     D-->
        //       ^
        //       1
        //
        return 1;
    }
    else if ((currDir == destDir) &&
             (currToDestDir != currDir) &&
             !(currToDestDir & reverseDestDir))
    {
        //
        //            2 > o        2 > o
        //
        //
        //                             D-->
        //
        return 2;
    }
    else if (currDir == reverseDestDir &&
             (currToDestDir != destDir) &&
             (currToDestDir != currDir))
    {
        //
        //                o < 2        o < 2       o < 2
        //
        //
        //                             D-->
        //
        return 2;
    }
    else if (currDirPerpendicularToDestDir &&
             (currToDestDir != (destDir | currDir)) &&
             (currToDestDir != currDir))
    {
        //
        //                                          3
        //                                          v
        //                o            o            o
        //                ^            ^            ^
        //                3            3            3
        //
        //                             D-->                 o
        //                                                  ^
        //                                                  3
        //
        return 3;
    }
    else if ((currDir == reverseDestDir) &&
             ((currToDestDir == destDir) || (currToDestDir == currDir)))
    {
        //
        //
        //
        //       o < 4                 D-->                 o < 4
        //
        return 4;
    }
    else if ((currDir == destDir) &&
             (currToDestDir & reverseDestDir))
    {
        //
        //                                      4 > o
        //
        //
        //                             D-->             4 > o
        //
        return 4;
    }

    // Should not be possible to reach here.
    COLA_ASSERT(false);
    return 0;
}


static double estimatedCostSpecific(ConnRef *lineRef, const Point *last,
        const Point& curr, const VertInf *costTar,
        const unsigned int costTarDirs)
{
    Point costTarPoint = costTar->point;

    if (lineRef->routingType() == ConnType_PolyLine)
    {
        return euclideanDist(curr, costTarPoint);
    }
    else // Orthogonal
    {
        // Really doesn't make sense to route orthogonal paths without
        // a segment penalty.
        COLA_ASSERT(lineRef->router()->routingParameter(segmentPenalty) > 0);

        double dist = manhattanDist(curr, costTarPoint);

        int bendCount = 0;
        double xmove = costTarPoint.x - curr.x;
        double ymove = costTarPoint.y - curr.y;
        if (last == nullptr)
        {
            // This is just the initial point.  Penalise it simply if it is
            // not inline with the target in either the x- or y-dimension.
            if ((xmove != 0) && (ymove != 0))
            {
                bendCount += 1;
            }
        }
        else if (dist > 0)
        {
            // We have two points and a non-zero distance, so we know
            // the segment direction.

            unsigned int currDir = orthogonalDirection(*last, curr);
            if ((currDir > 0) && (orthogonalDirectionsCount(currDir) == 1))
            {
                // Suitably high value, then we find the minimum.
                bendCount = 10;

                // Find the minimum bent penalty given all the possible
                // directions at the target point.
                if (costTarDirs & CostDirectionN)
                {
                    bendCount = std::min(bendCount,
                            bends(curr, currDir, costTarPoint, CostDirectionN));
                }
                if (costTarDirs & CostDirectionE)
                {
                    bendCount = std::min(bendCount,
                            bends(curr, currDir, costTarPoint, CostDirectionE));
                }
                if (costTarDirs & CostDirectionS)
                {
                    bendCount = std::min(bendCount,
                            bends(curr, currDir, costTarPoint, CostDirectionS));
                }
                if (costTarDirs & CostDirectionW)
                {
                    bendCount = std::min(bendCount,
                            bends(curr, currDir, costTarPoint, CostDirectionW));
                }
            }
        }
        double penalty = bendCount *
                lineRef->router()->routingParameter(segmentPenalty);

        return dist + penalty;
    }
}


double AStarPathPrivate::estimatedCost(ConnRef *lineRef, const Point *last,
        const Point& curr) const
{
    double estimate = DBL_MAX;
    COLA_ASSERT(m_cost_targets.size() > 0);

    // Find the minimum cost from the estimates to each of the possible
    // target points from this current point.
    for (size_t i = 0; i < m_cost_targets.size(); ++i)
    {
        double iEstimate = estimatedCostSpecific(lineRef, last,
                curr, m_cost_targets[i], m_cost_targets_directions[i]);

        // Add on the distance to the real target, otherwise this difference
        // might may make the comparisons unfair if they vary between targets.
        iEstimate += m_cost_targets_displacements[i];

        estimate = std::min(estimate, iEstimate);
    }
    return estimate;
}


class CmpVisEdgeRotation
{
    public:
        CmpVisEdgeRotation(const VertInf* lastPt)
            : _lastPt(lastPt)
        {
        }
        bool operator() (const EdgeInf* u, const EdgeInf* v) const
        {
            // Dummy ShapeConnectionPin edges are not orthogonal and
            // therefore can't be compared in the same way.
            if (u->isOrthogonal() && v->isOrthogonal())
            {
                return u->rotationLessThan(_lastPt, v);
            }
            return u < v;
        }
    private:
        const VertInf *_lastPt;
};


static inline bool pointAlignedWithOneOf(const Point& point,
        const std::vector<Point>& points, const size_t dim)
{
    for (size_t i = 0; i < points.size(); ++i)
    {
        if (point[dim] == points[i][dim])
        {
            return true;
        }
    }
    return false;
}

AStarPath::AStarPath(void)
    : m_private(new AStarPathPrivate())
{
}

AStarPath::~AStarPath(void)
{
    delete m_private;
}

void AStarPath::search(ConnRef *lineRef, VertInf *src, VertInf *tar, VertInf *start)
{
    m_private->search(lineRef, src, tar, start);
}

void AStarPathPrivate::determineEndPointLocation(double dist, VertInf *start,
        VertInf *target, VertInf *other, int level)
{
    COLA_UNUSED(dist);
    COLA_UNUSED(start);
    COLA_UNUSED(level);

    Point otherPoint = other->point;
    unsigned int thisDirs = orthogonalDirection(otherPoint, target->point);
    COLA_ASSERT(orthogonalDirectionsCount(thisDirs) > 0);
    double displacement = manhattanDist(otherPoint, target->point);

    m_cost_targets.push_back(other);
    m_cost_targets_directions.push_back(thisDirs);
    m_cost_targets_displacements.push_back(displacement);

#ifdef ESTIMATED_COST_DEBUG
    fprintf(stderr," - %g %g ", otherPoint.x, otherPoint.y);
    if (manhattanDist(start->point, otherPoint) > dist)
    {
        fprintf(stderr,"far ");
    }
    fprintf(stderr, "%s", (level == 1) ? "--" : "- ");
    printDirections(stderr, thisDirs);
    fprintf(stderr,"\n");
#endif
}

// Returns the best path from src to tar using the cost function.
//
// The path is worked out using the aStar algorithm, and is encoded via
// prevNode values for each ANode which point back to the previous ANode.
// At completion, this order is written into the pathNext links in each
// of the VerInfs along the path.
//
// The aStar STL code is originally based on public domain code available
// on the internet.
//
void AStarPathPrivate::search(ConnRef *lineRef, VertInf *src, VertInf *tar, VertInf *start)
{
    ANodeCmp pendingCmp;

    bool isOrthogonal = (lineRef->routingType() == ConnType_Orthogonal);

    if (start == nullptr)
    {
        start = src;
    }

#ifdef DEBUGHANDLER
    if (lineRef->router()->debugHandler())
    {
        lineRef->router()->debugHandler()->beginningSearchWithEndpoints(start, tar);
    }
#endif

    // Find a target point to use for cost estimate for orthogonal routing.
    //
    // If the connectivity is only on the far side we need to estimate to the
    // point on the far side.  Otherwise for orthogonal routing we can explore
    // all the space in between before we pay the extra cost to explore this
    // area.  This is especially true given many orthogonal routes have
    // equivalent costs.
#ifdef ESTIMATED_COST_DEBUG
    fprintf(stderr,"== aStar  %g %g ==\n", tar->point.x, tar->point.y);
#endif
    if (isOrthogonal && tar->id.isConnPt() && !tar->id.isConnCheckpoint())
    {
        // The target is a connector endpoint and the connector is orthogonal.
        double dist = manhattanDist(start->point, tar->point);
        for (EdgeInfList::const_iterator it = tar->orthogVisList.begin();
                it != tar->orthogVisList.end(); ++it)
        {
            // For each edge from the target endpoint, find the other vertex.
            EdgeInf *edge = *it;
            VertInf *other = edge->otherVert(tar);
            if (other->id.isConnectionPin())
            {
                // If this is a connection pin we need to do this process
                // another time since the current edge will be a dummy
                // zero-length edge.
                VertInf *replacementTar = other;
                for (EdgeInfList::const_iterator it =
                        replacementTar->orthogVisList.begin();
                        it != replacementTar->orthogVisList.end(); ++it)
                {
                    EdgeInf *edge = *it;
                    VertInf *other = edge->otherVert(replacementTar);
                    if ((other == tar) ||
                            (other->point == tar->point))
                    {
                        // Ignore edge we came from, or zero-length edges.
                        continue;
                    }

                    // Determine possible target endpoint directions and
                    // position.
                    determineEndPointLocation(dist, start, replacementTar,
                            other, 2);
                }
                continue;
            }

            // Determine possible target endpoint directions and position.
            determineEndPointLocation(dist, start, tar, other, 1);
        }
    }


    if (m_cost_targets.empty())
    {
        m_cost_targets.push_back(tar);
        // For polyline routing, assume target has visibility is all
        // directions for the purpose of cost estimations.
        m_cost_targets_directions.push_back(CostDirectionN |
                CostDirectionE | CostDirectionS | CostDirectionW);
        m_cost_targets_displacements.push_back(0.0);
    }

#ifdef ESTIMATED_COST_DEBUG
    fprintf(stderr, "------------\n");
    for (size_t i = 0; i < m_cost_targets.size(); ++i)
    {
        fprintf(stderr,"== %g %g - ", m_cost_targets[i]->point.x,
                m_cost_targets[i]->point.y);
        printDirections(stderr, m_cost_targets_directions[i]);
        fprintf(stderr,"\n");
    }
#endif


    double (*dist)(const Point& a, const Point& b) =
        (isOrthogonal) ? manhattanDist : euclideanDist;

    // We need to know the possible endpoints for doing an orthogonal
    // routing optimisation where we only turn when we are heading beside
    // a shape or are in line with a possible endpoint.
    std::vector<Point> endPoints;
    if (isOrthogonal)
    {
        endPoints = lineRef->possibleDstPinPoints();
    }
    endPoints.push_back(tar->point);

    // Heap of PENDING nodes.
    std::vector<ANode *> PENDING;
    PENDING.reserve(1000);

    size_t exploredCount = 0;
    ANode node, ati;
    ANode *bestNode = nullptr;         // Temporary bestNode
    bool bNodeFound = false;        // Flag if node is found in container
    int timestamp = 1;

    Router *router = lineRef->router();
    if (router->RubberBandRouting && (start != src))
    {
        COLA_ASSERT(router->IgnoreRegions == true);

        const PolyLine& currRoute = lineRef->route();
        VertInf *last = nullptr;
        int rIndx = 0;
        while (last != start)
        {
            const Point& pnt = currRoute.at(rIndx);
            VertIDProps props = (rIndx > 0) ? 0 : VertID::PROP_ConnPoint;
            VertID vID(pnt.id, pnt.vn, props);

#ifdef PATHDEBUG
            db_printf("/// %d %d\n", pnt.id, pnt.vn);
#endif
            VertInf *curr = router->vertices.getVertexByID(vID);
            COLA_ASSERT(curr != nullptr);

            node = ANode(curr, timestamp++);
            if (!last)
            {
                node.inf = src;
                node.g = 0;
                node.h = estimatedCost(lineRef, nullptr, node.inf->point);

                node.f = node.g + node.h;
            }
            else
            {
                double edgeDist = dist(bestNode->inf->point, curr->point);

                node.g = bestNode->g + cost(lineRef, edgeDist, bestNode->inf,
                        node.inf, bestNode->prevNode);

                // Calculate the Heuristic.
                node.h = estimatedCost(lineRef, &(bestNode->inf->point),
                        node.inf->point);

                // The A* formula
                node.f = node.g + node.h;

                // Point parent to last bestNode
                node.prevNode = bestNode;
            }

            if (curr != start)
            {
                bool addToPending = false;
                bestNode = newANode(node, addToPending);
                bestNode->inf->aStarDoneNodes.push_back(bestNode);
                ++exploredCount;
            }
            else
            {
                ANode * newNode = newANode(node);
                PENDING.push_back(newNode);
            }

            rIndx++;
            last = curr;
        }
    }
    else
    {
        if (start->pathNext)
        {
            // If we are doing checkpoint routing and have already done one
            // path, then we have an existing segment to consider for the
            // cost of the  choice from the start node, so we add a dummy
            // nodes as if they were already in the Done set.  This causes
            // us to first search in a collinear direction from the previous
            // segment.
            bool addToPending = false;
            bestNode = newANode(ANode(start->pathNext, timestamp++),
                    addToPending);
            bestNode->inf->aStarDoneNodes.push_back(bestNode);
            ++exploredCount;
        }

        // Create the start node
        node = ANode(src, timestamp++);
        node.g = 0;
        node.h = estimatedCost(lineRef, nullptr, node.inf->point);
        node.f = node.g + node.h;
        // Set a nullptr parent, so cost function knows this is the first segment.
        node.prevNode = bestNode;

        // Populate the PENDING container with the first location
        ANode *newNode = newANode(node);
        PENDING.push_back(newNode);
    }

    tar->pathNext = nullptr;

    // Create a heap from PENDING for sorting
    using std::make_heap; using std::push_heap; using std::pop_heap;
    make_heap( PENDING.begin(), PENDING.end(), pendingCmp);

    // Continue until the queue is empty.
    while (!PENDING.empty())
    {
        TIMER_VAR_ADD(router, 0, 1);
        // Set the Node with lowest f value to BESTNODE.
        // Since the ANode operator< is reversed, the head of the
        // heap is the node with the lowest f value.
        bestNode = PENDING.front();
        VertInf *bestNodeInf = bestNode->inf;

#ifdef DEBUGHANDLER
        if (router->debugHandler())
        {
            PolyLine currentSearchPath;

            ANode *curr = bestNode;
            while (curr)
            {
                currentSearchPath.ps.push_back(curr->inf->point);
                curr = curr->prevNode;
            }
            router->debugHandler()->updateCurrentSearchPath(currentSearchPath);
        }
#endif

        // Remove this node from the aStarPendingList
        std::list<ANode *>::iterator finishIt =
                bestNodeInf->aStarPendingNodes.end();
        for (std::list<ANode *>::iterator currInd =
                bestNodeInf->aStarPendingNodes.begin(); currInd != finishIt;
                ++currInd)
        {
            if (*currInd == bestNode)
            {
                bestNodeInf->aStarPendingNodes.erase(currInd);
                break;
            }
        }

        // Pop off the heap.  Actually this moves the
        // far left value to the far right.  The node
        // is not actually removed since the pop is to
        // the heap and not the container.
        pop_heap(PENDING.begin(), PENDING.end(), pendingCmp);
        // Remove node from right (the value we pop_heap'd)
        PENDING.pop_back();

        // Add the bestNode into the Done set.
        bestNodeInf->aStarDoneNodes.push_back(bestNode);
        ++exploredCount;

        VertInf *prevInf = (bestNode->prevNode) ? bestNode->prevNode->inf : nullptr;
#ifdef ASTAR_DEBUG
        db_printf("Considering... ");
        db_printf(" %g %g  ", bestNodeInf->point.x, bestNodeInf->point.y);
        bestNodeInf->id.db_print();
        db_printf(" - g: %3.1f h: %3.1f back: ", bestNode->g, bestNode->h);
        if (prevInf)
        {
            db_printf(" %g %g", prevInf->point.x, prevInf->point.y);
            //prevInf->id.db_print();
        }
        db_printf("\n");
#endif

        if (bestNodeInf == tar)
        {
            TIMER_VAR_ADD(router, 1, PENDING.size());
            // This node is our goal.
#ifdef ASTAR_DEBUG
            db_printf("LINE %10d  Steps: %4d  Cost: %g\n", lineRef->id(),
                    (int) exploredCount, bestNode->f);
#endif

            // Correct all the pathNext pointers.
            for (ANode *curr = bestNode; curr->prevNode; curr = curr->prevNode)
            {
#ifdef ASTAR_DEBUG
                db_printf("[%.12f, %.12f]\n", curr->inf->point.x, curr->inf->point.y);
#endif
                curr->inf->pathNext = curr->prevNode->inf;
            }
#ifdef ASTAR_DEBUG
            db_printf("\n");
#endif

            // Exit from the search
            break;
        }

        // Check adjacent points in graph and add them to the queue.
        EdgeInfList& visList = (!isOrthogonal) ?
                bestNodeInf->visList : bestNodeInf->orthogVisList;
        if (isOrthogonal)
        {
            // We would like to explore in a structured way,
            // so sort the points in the visList...
            CmpVisEdgeRotation compare(prevInf);
            visList.sort(compare);
        }
        EdgeInfList::const_iterator finish = visList.end();
        for (EdgeInfList::const_iterator edge = visList.begin();
                edge != finish; ++edge)
        {
            if ((*edge)->isDisabled())
            {
                // Skip disabled edges.
                continue;
            }

            node = ANode((*edge)->otherVert(bestNodeInf), timestamp++);

            // Set the index to the previous ANode that we reached
            // this ANode via.
            node.prevNode = bestNode;

            VertInf *prevInf = (bestNode->prevNode) ?
                    bestNode->prevNode->inf : nullptr;

            // Don't bother looking at the segment we just arrived along.
            if (prevInf && (prevInf == node.inf))
            {
                continue;
            }
            if (node.inf->id.isConnectionPin() &&
                    !node.inf->id.isConnCheckpoint())
            {
                if ( !( (bestNodeInf == lineRef->src()) &&
                        lineRef->src()->id.isDummyPinHelper()
                      ) &&
                     !( node.inf->hasNeighbour(lineRef->dst(), isOrthogonal) &&
                        lineRef->dst()->id.isDummyPinHelper())
                      )
                {
                    // Don't check connection pins if they don't have the
                    // target vertex as a direct neighbour, or are directly
                    // leaving the source vertex.
                    continue;
                }
            }
            else if (node.inf->id.isConnPt())
            {
                if ((node.inf != tar))
                {
                    // Don't check connector endpoints vertices unless they
                    // are the target endpoint.
                    continue;
                }
            }

            if (isOrthogonal && !(*edge)->isDummyConnection())
            {
                // Orthogonal routing optimisation.
                // Skip the edges that don't lead to shape edges, or the
                // connection point we are looking for.  Though allow them
                // if we haven't yet turned from the source point, since it
                // may be a free-floating endpoint with directional visibility.
                // Also, don't check if the previous point was a dummy for a
                // connection pin and this happens to be placed diagonally
                // from here, i.e., when both of notInline{X,Y} are true.
                Point& bestPt = bestNodeInf->point;
                Point& nextPt = node.inf->point;

                bool notInlineX = prevInf && (prevInf->point.x != bestPt.x);
                bool notInlineY = prevInf && (prevInf->point.y != bestPt.y);
                if ((bestPt.x == nextPt.x) && notInlineX && !notInlineY &&
                        (bestPt[YDIM] != src->point[YDIM]))
                {
                    if (nextPt.y < bestPt.y)
                    {
                        if (!(bestNodeInf->orthogVisPropFlags & YL_EDGE) &&
                                !pointAlignedWithOneOf(bestPt, endPoints, XDIM))
                        {
                            continue;
                        }
                    }
                    else if (nextPt.y > bestPt.y)
                    {
                        if (!(bestNodeInf->orthogVisPropFlags & YH_EDGE) &&
                                !pointAlignedWithOneOf(bestPt, endPoints, XDIM))
                        {
                            continue;
                        }
                    }
                }
                if ((bestPt.y == nextPt.y) && notInlineY && !notInlineX &&
                        (bestPt[XDIM] != src->point[XDIM]))
                {
                    if (nextPt.x < bestPt.x)
                    {
                        if (!(bestNodeInf->orthogVisPropFlags & XL_EDGE) &&
                                !pointAlignedWithOneOf(bestPt, endPoints, YDIM))
                        {
                            continue;
                        }
                    }
                    else if (nextPt.x > bestPt.x)
                    {
                        if (!(bestNodeInf->orthogVisPropFlags & XH_EDGE) &&
                                !pointAlignedWithOneOf(bestPt, endPoints, YDIM))
                        {
                            continue;
                        }
                    }
                }
            }

            double edgeDist = (*edge)->getDist();

            if (edgeDist == 0)
            {
                continue;
            }

            if (!isOrthogonal &&
                  (!router->RubberBandRouting || (start == src)) &&
                  (validateBendPoint(prevInf, bestNodeInf, node.inf) == false))
            {
                // The bendpoint is not valid, i.e., is a zigzag corner, so...
                continue;
                // For RubberBand routing we want to allow these routes and
                // unwind them later, otherwise instead or unwinding, paths
                // can go the *really* long way round.
            }

            // Figure out if we are at one of the cost targets.
            bool atCostTarget = false;
            for (size_t i = 0; i < m_cost_targets.size(); ++i)
            {
                if (bestNode->inf == m_cost_targets[i])

                {
                    atCostTarget = true;
                    break;
                }
            }

            if (atCostTarget &&
                    (node.inf->id.isConnectionPin() || (node.inf == tar)))
            {
                // This is a point on the side of an obstacle that connects
                // to the target or a connection pin.  It should have no
                // further cost and the heuristic should be zero.
                node.g = bestNode->g;
                node.h = 0;
            }
            else
            {
                if (node.inf == tar)
                {
                    // We've reached the target.  The heuristic should be zero.
                    node.h = 0;
                }
                else
                {
                    // Otherwise, calculate the heuristic value.
                    node.h = estimatedCost(lineRef, &(bestNodeInf->point),
                            node.inf->point);
                }

                if (node.inf->id.isDummyPinHelper())
                {
                    // This is connecting to a connection pin helper vertex.
                    // There should be no additional cost for this step.
                    node.g = bestNode->g;
                }
                else
                {
                    // Otherwise, calculate the cost of this step.
                    node.g = bestNode->g + cost(lineRef, edgeDist, bestNodeInf,
                            node.inf, bestNode->prevNode);
                }
            }

            // The A* formula
            node.f = node.g + node.h;

#ifdef ASTAR_DEBUG
            db_printf("-- Adding: %g %g  ", node.inf->point.x,
                    node.inf->point.y);
            node.inf->id.db_print();
            db_printf(" - g: %3.1f h: %3.1f \n", node.g, node.h);
#endif

            bNodeFound = false;


            // Check to see if already on PENDING
            std::list<ANode *>::const_iterator finish = node.inf->aStarPendingNodes.end();
            for (std::list<ANode *>::const_iterator currInd =
                    node.inf->aStarPendingNodes.begin(); currInd != finish; ++currInd)
            {
                ati = **currInd;
                // The (node.prevNode == ati.prevNode) is redundant, but may
                // save checking the mosre costly prevNode->inf test if the
                // Nodes are the same.
                if ((node.inf == ati.inf) &&
                        ((node.prevNode == ati.prevNode) ||
                         (node.prevNode->inf == ati.prevNode->inf)))
                {
                    // If already on PENDING
                    if (node.g < ati.g)
                    {
                        // Replace the existing node in PENDING
                        **currInd = node;
                        make_heap( PENDING.begin(), PENDING.end(), pendingCmp);
                    }
                    bNodeFound = true;
                    break;
                }
            }
            if ( !bNodeFound ) // If Node NOT found on PENDING
            {
                // Check to see if it is already in the Done set for this
                // vertex.
                for (std::list<ANode *>::const_iterator currInd =
                        node.inf->aStarDoneNodes.begin();
                        currInd != node.inf->aStarDoneNodes.end(); ++currInd)
                {
                    ati = **currInd;
                    // The (node.prevNode == ati.prevNode) is redundant, but may
                    // save checking the mosre costly prevNode->inf test if the
                    // Nodes are the same.
                    if ((node.inf == ati.inf) && ati.prevNode &&
                            ((node.prevNode == ati.prevNode) ||
                             (node.prevNode->inf == ati.prevNode->inf)))
                    {
                        //COLA_ASSERT(node.g >= (ati.g - 10e-10));
                        // This node is already in the Done set and the
                        // current node also has a higher g-value, so we
                        // don't need to consider this node.
                        bNodeFound = true;
                        break;
                    }
                }
            }

            if (!bNodeFound ) // If Node NOT in either Pending or Done.
            {
                // Push NewNode onto PENDING
                ANode *newNode = newANode(node);
                PENDING.push_back(newNode);
                // Push NewNode onto heap
                push_heap( PENDING.begin(), PENDING.end(), pendingCmp);

#if 0
                using std::cout; using std::endl;
                // Display PENDING container (For Debugging)
                cout << "PENDING:   ";
                for (unsigned int i = 0; i < PENDING.size(); i++)
                {
                    cout << PENDING[i]->g << "," << PENDING[i]->h << ",";
                    cout << PENDING[i]->inf << "," << PENDING[i]->pp << "  ";
                }
                cout << endl << endl;
#endif
            }
        }
    }

    // Cleanup lists used to store Done and Pending sets for each vertex.
    VertInf *endVert = router->vertices.end();
    for (VertInf *k = router->vertices.connsBegin(); k != endVert;
            k = k->lstNext)
    {
        k->aStarDoneNodes.clear();
        k->aStarPendingNodes.clear();
    }
}


}