/*
  * @param target1 the first of two possible target segments for the transfer
  * @param target2 the second of two possible target segments for the
  * transfer
  * @param ignore the constraint which, in being satisfied, caused this
  * transfer and which should therefore not be transfered.
  */
 transferStraightConstraintChoose(Segment* target1, Segment* target2,
         StraightConstraint* ignore)
     : ignore(ignore) 
 {
     vpsc::Dim dim = ignore->scanDim;
     double min1=min(target1->start->pos(vpsc::conjugate(dim)),target1->end->pos(vpsc::conjugate(dim)));
     double max1=max(target1->start->pos(vpsc::conjugate(dim)),target1->end->pos(vpsc::conjugate(dim)));
     double min2=min(target2->start->pos(vpsc::conjugate(dim)),target2->end->pos(vpsc::conjugate(dim)));
     double max2=max(target2->start->pos(vpsc::conjugate(dim)),target2->end->pos(vpsc::conjugate(dim)));
     if(min1<max2) {
         COLA_ASSERT(max1==min2);
         lSeg = target1;
         rSeg = target2;
         lMin = min1;
         mid = max1;
         rMax = max2;
     } else {
         COLA_ASSERT(max2==min1);
         lSeg = target2;
         rSeg = target1;
         lMin = min2;
         mid = max2;
         rMax = max1;
     }
 }

static double hRule8(vpsc::Dim dim, const EdgePoint* u, const EdgePoint* v,
        const EdgePoint* w, const EdgePoint* a, const EdgePoint* b,
        const EdgePoint* c)
{
    double dxuv, dyuv, dxuv2, dyuv2;
    double luv=dim==vpsc::HORIZONTAL?
        len(u,v,dxuv,dyuv,dxuv2,dyuv2):
        len(u,v,dyuv,dxuv,dyuv2,dxuv2);
    COLA_ASSERT(luv!=0);
    double dxvw, dyvw, dxvw2, dyvw2;
    double lvw=dim==vpsc::HORIZONTAL?
        len(v,w,dxvw,dyvw,dxvw2,dyvw2):
        len(v,w,dyvw,dxvw,dyvw2,dxvw2);
    COLA_ASSERT(lvw!=0);
    double dxab, dyab, dxab2, dyab2;
    double lab=dim==vpsc::HORIZONTAL?
        len(a,b,dxab,dyab,dxab2,dyab2):
        len(a,b,dyab,dxab,dyab2,dxab2);
    COLA_ASSERT(lab!=0);
    double dxbc, dybc, dxbc2, dybc2;
    double lbc=dim==vpsc::HORIZONTAL?
        len(b,c,dxbc,dybc,dxbc2,dybc2):
        len(b,c,dybc,dxbc,dybc2,dxbc2);
    COLA_ASSERT(lbc!=0);
    return (dxuv/luv - dxvw/lvw) * (dxab/lab - dxbc/lbc);
}

// Returns the rotationalAngle, between 0 and 360, of this point from (0,0).
//
double rotationalAngle(const Point& p)
{
    if (p.y == 0)
    {
        return ((p.x < 0) ? 180 : 0);
    }
    else if (p.x == 0)
    {
        return ((p.y < 0) ? 270 : 90);
    }
    
    double ang = atan(p.y / p.x);
    ang = (ang * 180) / M_PI;

    if (p.x < 0)
    {
        ang += 180;
    }
    else if (p.y < 0)
    {
        ang += 360;
    }
    COLA_ASSERT(ang >= 0);
    COLA_ASSERT(ang <= 360);

    return ang;
}

Obstacle::~Obstacle()
{
    COLA_ASSERT(!m_router->objectIsInQueuedActionList(this));

    if (m_active)
    {
        makeInactive();
        m_router->processTransaction();
    }

    COLA_ASSERT(m_first_vert != NULL);
    
    VertInf *it = m_first_vert;
    do
    {
        VertInf *tmp = it;
        it = it->shNext;

        delete tmp;
    }
    while (it != m_first_vert);
    m_first_vert = m_last_vert = NULL;

    // Free and clear any connection pins.
    while (!m_connection_pins.empty())
    {
        delete *(m_connection_pins.begin());
    }
}

void Obstacle::setNewPoly(const Polygon& poly)
{
    COLA_ASSERT(m_first_vert != NULL);
    COLA_ASSERT(m_polygon.size() == poly.size());

    m_polygon = poly;
    Polygon routingPoly = routingPolygon();

    VertInf *curr = m_first_vert;
    for (size_t pt_i = 0; pt_i < routingPoly.size(); ++pt_i)
    {
        COLA_ASSERT(curr->visListSize == 0);
        COLA_ASSERT(curr->invisListSize == 0);

        // Reset with the new polygon point.
        curr->Reset(routingPoly.ps[pt_i]);
        curr->pathNext = NULL;

        curr = curr->shNext;
    }
    COLA_ASSERT(curr == m_first_vert);

    // It may be that the polygon for the obstacle has been updated after
    // creating the shape.  These events may have been combined for a single
    // transaction, so update pin positions.
    for (ShapeConnectionPinSet::iterator curr =
                m_connection_pins.begin(); curr != m_connection_pins.end(); ++curr)
    {
        ShapeConnectionPin *pin = *curr;
        pin->updatePosition(m_polygon);
    }
}

void floyd_warshall(
        unsigned const n,
        T** D, 
        vector<Edge> const & es,
        valarray<T> const * eweights) 
{
    COLA_ASSERT(!eweights||eweights->size()==es.size());
    for(unsigned i=0;i<n;i++) {
        for(unsigned j=0;j<n;j++) {
            if(i==j) D[i][j]=0;
            else D[i][j]=numeric_limits<T>::max();
        }
    }
    for(unsigned i=0;i<es.size();i++) {
        unsigned u=es[i].first, v=es[i].second;
        COLA_ASSERT(u<n&&v<n);
        D[u][v]=D[v][u]=eweights?(*eweights)[i]:1;
    }
    for(unsigned k=0; k<n; k++) {
        for(unsigned i=0; i<n; i++) {
            for(unsigned j=0; j<n; j++) {
                D[i][j]=min(D[i][j],D[i][k]+D[k][j]);
            }
        }
    }
}

ReferencingPolygon::ReferencingPolygon(const Polygon& poly, const Router *router)
    : PolygonInterface(),
      _id(poly._id),
      psRef(poly.size()),
      psPoints(poly.size())
{
    COLA_ASSERT(router != NULL);
    for (size_t i = 0; i < poly.size(); ++i)
    {
        if (poly.ps[i].id == 0)
        {
            // Can't be referenced, so just make a copy of the point.
            psRef[i] = std::make_pair((Polygon *) NULL, 
                    kUnassignedVertexNumber);
            psPoints[i] = poly.ps[i];
        }
        else
        {
            const Polygon *polyPtr = NULL;
            for (ObstacleList::const_iterator sh = router->m_obstacles.begin();
                    sh != router->m_obstacles.end(); ++sh) 
            {
                if ((*sh)->id() == poly.ps[i].id)
                {
                    const Polygon& poly = (*sh)->polygon();
                    polyPtr = &poly;
                    break;
                }
            }
            COLA_ASSERT(polyPtr != NULL);
            psRef[i] = std::make_pair(polyPtr, poly.ps[i].vn);
        }
    }
}

void VertInfList::addVertex(VertInf *vert)
{
    checkVertInfListConditions();
    COLA_ASSERT(vert->lstPrev == NULL);
    COLA_ASSERT(vert->lstNext == NULL);

    if (vert->id.isConnPt())
    {
        // A Connector vertex
        if (_firstConnVert)
        {
            // Join with previous front
            vert->lstNext = _firstConnVert;
            _firstConnVert->lstPrev = vert;

            // Make front
            _firstConnVert = vert;
        }
        else
        {
            // Make front and back
            _firstConnVert = vert;
            _lastConnVert = vert;

            // Link to front of shapes list
            vert->lstNext = _firstShapeVert;
        }
        _connVertices++;
    }
    else // if (vert->id.shape > 0)
    {
        // A Shape vertex
        if (_lastShapeVert)
        {
            // Join with previous back
            vert->lstPrev = _lastShapeVert;
            _lastShapeVert->lstNext = vert;

            // Make back
            _lastShapeVert = vert;
        }
        else
        {
            // Make first and last
            _firstShapeVert = vert;
            _lastShapeVert = vert;

            // Join with conns list
            if (_lastConnVert)
            {
                COLA_ASSERT(_lastConnVert->lstNext == NULL);

                _lastConnVert->lstNext = vert;
            }
        }
        _shapeVertices++;
    }
    checkVertInfListConditions();
}

// Creates the connection between a connector and a shape/junction.
void ConnEnd::connect(ConnRef *conn)
{
    COLA_ASSERT(isPinConnection());
    COLA_ASSERT(m_anchor_obj);
    COLA_ASSERT(m_conn_ref == NULL);

    m_anchor_obj->addFollowingConnEnd(this);
    m_conn_ref = conn;
}

void HyperedgeTreeNode::validateHyperedge(const HyperedgeTreeEdge *ignored, 
        const size_t dist) const 
{
    size_t newDist = dist;
#ifdef MAJOR_HYPEREDGE_IMPROVEMENT_DEBUG
    if (junction)
    {
        if (newDist == 0)
        {
            fprintf(stderr,"\nHyperedge topology:\n");
        }
        else
        {
            ++newDist;
        }
        for (size_t d = 0; d < newDist; ++d)
        {
            fprintf(stderr,"  ");
        }
        fprintf(stderr, "j(%d)\n", junction->id());
        ++newDist;
    }
    else if (edges.size() == 1)
    {
        ++newDist;
        for (size_t d = 0; d < newDist; ++d)
        {
            fprintf(stderr, "  ");
        }
        fprintf(stderr, "t()\n");
        ++newDist;
    }
#endif
    for (std::list<HyperedgeTreeEdge *>::const_iterator curr = edges.begin();
            curr != edges.end(); ++curr)
    {
        HyperedgeTreeEdge *edge = *curr;
        std::pair<ConnEnd, ConnEnd> connEnds = edge->conn->endpointConnEnds();

        if (junction)
        {
            COLA_ASSERT((connEnds.first.junction() == junction) ||
                        (connEnds.second.junction() == junction));
            COLA_ASSERT(connEnds.first.junction() != connEnds.second.junction());
        }
        else if (edges.size() == 1)
        {
            COLA_ASSERT(!connEnds.first.junction() || 
                        !connEnds.second.junction());
        }
        
        if (edge != ignored)
        {
            edge->validateHyperedge(this, newDist);
        }
    }
}

// This method disconnects the hyperedge tree edge nodes that it's attached to.
//
void HyperEdgeTreeEdge::disconnectEdge(void)
{
    COLA_ASSERT(ends.first != NULL);
    COLA_ASSERT(ends.second != NULL);

    ends.first->disconnectEdge(this);
    ends.second->disconnectEdge(this);
    ends.first = NULL;
    ends.second = NULL;
}

void dijkstra(
        unsigned const s,
        unsigned const n,
        T* d,
        vector<Edge> const & es,
        valarray<T> const * eweights)
{
    COLA_ASSERT(!eweights||es.size()==eweights->size());
    COLA_ASSERT(s<n);
    vector<Node<T> > vs(n);
    dijkstra_init(vs,es,eweights);
    dijkstra(s,vs,d);
}

// Populate the deleted-object vectors with all the connectors and junctions
// that form the registered hyperedges.  Then return the set of all these
// connectors so they can be ignored for individual rerouting.
ConnRefSet HyperedgeRerouter::calcHyperedgeConnectors(void)
{
    COLA_ASSERT(m_router != NULL);

    ConnRefSet allRegisteredHyperedgeConns;

    // Clear the deleted-object vectors.  We populate them here if necessary.
    m_deleted_junctions_vector.clear();
    m_deleted_junctions_vector.resize(count());
    m_deleted_connectors_vector.clear();
    m_deleted_connectors_vector.resize(count());

    m_terminal_vertices_vector.clear();
    m_terminal_vertices_vector.resize(count());
    m_added_vertices.clear();

    // Populate the deleted-object vectors.
    const size_t num_hyperedges = count();
    for (size_t i = 0; i < num_hyperedges; ++i)
    {
        if (m_root_junction_vector[i])
        {
            // Follow objects attached to junction to find the hyperedge.
            findAttachedObjects(i, m_root_junction_vector[i], NULL,
                    allRegisteredHyperedgeConns);
            continue;
        }

        // Alternatively, we have a set of ConnEnds, so store the
        // corresponding terminals
        std::pair<bool, VertInf *> maybeNewVertex;
        for (ConnEndList::const_iterator it = m_terminals_vector[i].begin();
                it != m_terminals_vector[i].end(); ++it)
        {
            maybeNewVertex = it->getHyperedgeVertex(m_router);
            COLA_ASSERT(maybeNewVertex.second != NULL);
            m_terminal_vertices_vector[i].insert(maybeNewVertex.second);

            if (maybeNewVertex.first)
            {
                // This is a newly created vertex.  Remember it so we can
                // free it and it's visibility edges later.
                m_added_vertices.push_back(maybeNewVertex.second);
            }
        }
    }

    // Return these connectors that don't require rerouting.
    return allRegisteredHyperedgeConns;
}

static double hRule4(vpsc::Dim dim, const EdgePoint* a, const EdgePoint* b,
        const EdgePoint* c, const EdgePoint* d)
{
    double dxab, dyab, dxab2, dyab2;
    double lab=dim==vpsc::HORIZONTAL?
        len(a,b,dxab,dyab,dxab2,dyab2):
        len(a,b,dyab,dxab,dyab2,dxab2);
    COLA_ASSERT(lab!=0);
    double dxcd, dycd, dxcd2, dycd2;
    double lcd=dim==vpsc::HORIZONTAL?
        len(c,d,dxcd,dycd,dxcd2,dycd2):
        len(c,d,dycd,dxcd,dycd2,dxcd2);
    COLA_ASSERT(lcd!=0);
    return -dxab*dxcd/(lab*lcd);
}

ConnEnd::ConnEnd(ShapeRef *shapeRef, const unsigned int connectionPinClassID)
    : m_type(ConnEndShapePin),
      m_point(Point(0,0)),
      m_directions(ConnDirAll),
      m_connection_pin_class_id(connectionPinClassID),
      m_anchor_obj(shapeRef),
      m_conn_ref(NULL),
      m_active_pin(NULL)
{
    COLA_ASSERT(m_anchor_obj != NULL);
    COLA_ASSERT(m_connection_pin_class_id > 0);

    m_point = m_anchor_obj->position();
    COLA_ASSERT(m_connection_pin_class_id != CONNECTIONPIN_UNSET);
}

static double gRule2(vpsc::Dim dim, const EdgePoint* a, const EdgePoint* b,
        const EdgePoint* c)
{
    double dxab, dyab, dxab2, dyab2;
    double lab=dim==vpsc::HORIZONTAL?
        len(a,b,dxab,dyab,dxab2,dyab2):
        len(a,b,dyab,dxab,dyab2,dxab2);
    COLA_ASSERT(lab!=0);
    double dxbc, dybc, dxbc2, dybc2;
    double lbc=dim==vpsc::HORIZONTAL?
        len(b,c,dxbc,dybc,dxbc2,dybc2):
        len(b,c,dybc,dxbc,dybc2,dxbc2);
    COLA_ASSERT(lbc!=0);
    return dxab/lab - dxbc/lbc;
}

double GradientProjection::computeSteepestDescentVector(
        valarray<double> const &b,
        valarray<double> const &x,
        valarray<double> &g) const {
    // find steepest descent direction
    //  g = 2 ( b - A x )
    //    where: A = denseQ + sparseQ
    //  g = 2 ( b - denseQ x) - 2 sparseQ x
    //
    //  except the 2s don't matter because we compute 
    //  the optimal stepsize anyway
    COLA_ASSERT(x.size()==b.size() && b.size()==g.size());
    g = b;
    for (unsigned i=0; i<denseSize; i++) {
        for (unsigned j=0; j<denseSize; j++) {
            g[i] -= (*denseQ)[i*denseSize+j]*x[j];
        }
    }
    // sparse part:
    if(sparseQ) {
        valarray<double> r(x.size());
        sparseQ->rightMultiply(x,r);
        g-=r;
    }
    return computeStepSize(g,g);
}

bool zagzig(const EdgePoint* a, const Segment* s) {
    COLA_UNUSED(a);
    if(s!=nullptr) {
        COLA_ASSERT(!sameCorner(a,s->start));
    }
    return false;
}

bool sameCorner(const EdgePoint* a, const EdgePoint* b) {
    COLA_UNUSED(a);
    COLA_UNUSED(b);
    COLA_ASSERT( !(a->node->id==b->node->id
                &&a->rectIntersect==b->rectIntersect));
    return false;
}

ShapeConnectionPin::ShapeConnectionPin(JunctionRef *junction, 
        const unsigned int classId, const ConnDirFlags visDirs)
    : m_shape(NULL),
      m_junction(junction),
      m_class_id(classId),
      m_x_portion_offset(0.0),
      m_y_portion_offset(0.0),
      m_inside_offset(0.0),
      m_visibility_directions(visDirs),
      m_exclusive(true),
      m_connection_cost(0.0),
      m_vertex(NULL)
{
    COLA_ASSERT(m_junction != NULL);
    m_router = m_junction->router();
    m_junction->addConnectionPin(this);
    
    // Create a visibility vertex for this ShapeConnectionPin.
    VertID id(m_junction->id(), kShapeConnectionPin, 
            VertID::PROP_ConnPoint | VertID::PROP_ConnectionPin);
    m_vertex = new VertInf(m_router, id, m_junction->position());
    m_vertex->visDirections = visDirs;

    if (m_router->_polyLineRouting)
    {
        vertexVisibility(m_vertex, NULL, true, true);
    }
}