/*
 * vim: ts=4 sw=4 et tw=0 wm=0
 *
 * libavoid - Fast, Incremental, Object-avoiding Line Router
 *
 * Copyright (C) 2005-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):  Tim Dwyer
 *             Michael Wybrow
 *
 * --------------
 *
 * This file contains a slightly modified version of IncSolver() from libvpsc:
 * A solver for the problem of Variable Placement with Separation Constraints.
 * It has the following changes from the Adaptagrams VPSC version:
 *  -  The required VPSC code has been consolidated into a single file.
 *  -  Unnecessary code, like the Solver() class, has been removed.
 *  -  The PairingHeap code has been replaced by a STL priority_queue.
 *
 * Modifications:  Michael Wybrow
 *
*/

#include "libavoid/vpsc.h"

#ifndef USELIBVPSC

#include <iostream>
#include <cmath>
#include <sstream>
#include <map>
#include <cfloat>
#include <cstdio>

#include "libavoid/assertions.h"
#include "libavoid/debug.h"


using namespace std;

namespace Avoid {

static const double ZERO_UPPERBOUND=-1e-10;
static const double LAGRANGIAN_TOLERANCE=-1e-4;


IncSolver::IncSolver(Variables const &vs, Constraints const &cs)
    : m(cs.size()),
      cs(cs),
      n(vs.size()), 
      vs(vs),
      needsScaling(false)
{
    for(unsigned i=0;i<n;++i) {
        vs[i]->in.clear();
        vs[i]->out.clear();

        // Set needsScaling if any variables have a scale other than 1.
        needsScaling |= (vs[i]->scale != 1);
    }
    for(unsigned i=0;i<m;++i) {
        Constraint *c=cs[i];
        c->left->out.push_back(c);
        c->right->in.push_back(c);
        c->needsScaling = needsScaling;
    }
    bs=new Blocks(vs);
#ifdef LIBVPSC_LOGGING
    printBlocks();
    //COLA_ASSERT(!constraintGraphIsCyclic(n,vs));
#endif

    inactive=cs;
    for(Constraints::iterator i=inactive.begin();i!=inactive.end();++i) {
        (*i)->active=false;
    }
}
IncSolver::~IncSolver() {
    delete bs;
}

void IncSolver::addConstraint(Constraint *c)
{
    ++m;
    c->active = false;
    inactive.push_back(c);
    c->left->out.push_back(c);
    c->right->in.push_back(c);
    c->needsScaling = needsScaling;
}

// useful in debugging
void IncSolver::printBlocks() {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    for(set<Block*>::iterator i=bs->begin();i!=bs->end();++i) {
        Block *b=*i;
        f<<"  "<<*b<<endl;
    }
    for(unsigned i=0;i<m;i++) {
        f<<"  "<<*cs[i]<<endl;
    }
#endif
}

/*
 * Stores the relative positions of the variables in their finalPosition
 * field.
 */
void IncSolver::copyResult() {
    for(Variables::const_iterator i=vs.begin();i!=vs.end();++i) {
        Variable* v=*i;
        v->finalPosition=v->position();
        COLA_ASSERT(v->finalPosition==v->finalPosition);
    }
}

struct node {
    set<node*> in;
    set<node*> out;
};
// useful in debugging - cycles would be BAD
bool IncSolver::constraintGraphIsCyclic(const unsigned n, Variable* const vs[]) {
    map<Variable*, node*> varmap;
    vector<node*> graph;
    for(unsigned i=0;i<n;i++) {
        node *u=new node;
        graph.push_back(u);
        varmap[vs[i]]=u;
    }
    for(unsigned i=0;i<n;i++) {
        for(vector<Constraint*>::iterator c=vs[i]->in.begin();c!=vs[i]->in.end();++c) {
            Variable *l=(*c)->left;
            varmap[vs[i]]->in.insert(varmap[l]);
        }

        for(vector<Constraint*>::iterator c=vs[i]->out.begin();c!=vs[i]->out.end();++c) {
            Variable *r=(*c)->right;
            varmap[vs[i]]->out.insert(varmap[r]);
        }
    }
    while(graph.size()>0) {
        node *u=nullptr;
        vector<node*>::iterator i=graph.begin();
        for(;i!=graph.end();++i) {
            u=*i;
            if(u->in.size()==0) {
                break;
            }
        }
        if(i==graph.end() && graph.size()>0) {
            //cycle found!
            return true;
        } else {
            graph.erase(i);
            for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
                node *v=*j;
                v->in.erase(u);
            }
            delete u;
        }
    }
    for(unsigned i=0; i<graph.size(); ++i) {
        delete graph[i];
    }
    return false;
}

// useful in debugging - cycles would be BAD
bool IncSolver::blockGraphIsCyclic() {
    map<Block*, node*> bmap;
    vector<node*> graph;
    size_t length = bs->size();
    for (size_t i = 0; i < length; ++i)
    {
        Block *b = bs->at(i);
        node *u=new node;
        graph.push_back(u);
        bmap[b]=u;
    }
    for (size_t i = 0; i < length; ++i)
    {
        Block *b = bs->at(i);
        b->setUpInConstraints();
        Constraint *c=b->findMinInConstraint();
        while(c!=nullptr) {
            Block *l=c->left->block;
            bmap[b]->in.insert(bmap[l]);
            b->deleteMinInConstraint();
            c=b->findMinInConstraint();
        }

        b->setUpOutConstraints();
        c=b->findMinOutConstraint();
        while(c!=nullptr) {
            Block *r=c->right->block;
            bmap[b]->out.insert(bmap[r]);
            b->deleteMinOutConstraint();
            c=b->findMinOutConstraint();
        }
    }
    while(graph.size()>0) {
        node *u=nullptr;
        vector<node*>::iterator i=graph.begin();
        for(;i!=graph.end();++i) {
            u=*i;
            if(u->in.size()==0) {
                break;
            }
        }
        if(i==graph.end() && graph.size()>0) {
            //cycle found!
            return true;
        } else {
            graph.erase(i);
            for(set<node*>::iterator j=u->out.begin();j!=u->out.end();++j) {
                node *v=*j;
                v->in.erase(u);
            }
            delete u;
        }
    }
    for(unsigned i=0; i<graph.size(); i++) {
        delete graph[i];
    }
    return false;
}

bool IncSolver::solve() {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"solve_inc()..."<<endl;
#endif
    satisfy();
    double lastcost = DBL_MAX, cost = bs->cost();
    while(fabs(lastcost-cost)>0.0001) {
        satisfy();
        lastcost=cost;
        cost = bs->cost();
#ifdef LIBVPSC_LOGGING
        f<<"  bs->size="<<bs->size()<<", cost="<<cost<<endl;
#endif
    }
    copyResult();
    return bs->size()!=n; 
}
/*
 * incremental version of satisfy that allows refinement after blocks are
 * moved.
 *
 *  - move blocks to new positions
 *  - repeatedly merge across most violated constraint until no more
 *    violated constraints exist
 *
 * Note: there is a special case to handle when the most violated constraint
 * is between two variables in the same block.  Then, we must split the block
 * over an active constraint between the two variables.  We choose the 
 * constraint with the most negative lagrangian multiplier. 
 */
bool IncSolver::satisfy() {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"satisfy_inc()..."<<endl;
#endif
    splitBlocks();
    //long splitCtr = 0;
    Constraint* v = nullptr;
    //CBuffer buffer(inactive);
    while ( (v = mostViolated(inactive)) && 
            (v->equality || ((v->slack() < ZERO_UPPERBOUND) && !v->active)) )
    {
        COLA_ASSERT(!v->active);
        Block *lb = v->left->block, *rb = v->right->block;
        if(lb != rb) {
            lb->merge(rb,v);
        } else {
            if(lb->isActiveDirectedPathBetween(v->right,v->left)) {
                // cycle found, relax the violated, cyclic constraint
                v->unsatisfiable=true;
                continue;
                //UnsatisfiableException e;
                //lb->getActiveDirectedPathBetween(e.path,v->right,v->left);
                //e.path.push_back(v);
                //throw e;
            }
            //if(splitCtr++>10000) {
                //throw "Cycle Error!";
            //}
            // constraint is within block, need to split first
            try {
                Constraint* splitConstraint
                    =lb->splitBetween(v->left,v->right,lb,rb);
                if(splitConstraint!=nullptr) {
                    COLA_ASSERT(!splitConstraint->active);
                    inactive.push_back(splitConstraint);
                } else {
                    v->unsatisfiable=true;
                    continue;
                }
            } catch(UnsatisfiableException e) {
                e.path.push_back(v);
                /*
                std::cerr << "Unsatisfiable:" << std::endl;
                for(std::vector<Constraint*>::iterator r=e.path.begin();
                        r!=e.path.end();++r)
                {
                    std::cerr << **r <<std::endl;
                }
                */
                v->unsatisfiable=true;
                continue;
            }
            if(v->slack()>=0) {
                COLA_ASSERT(!v->active);
                // v was satisfied by the above split!
                inactive.push_back(v);
                bs->insert(lb);
                bs->insert(rb);
            } else {
                bs->insert(lb->merge(rb,v));
                delete ((lb->deleted) ? lb : rb);
            }
        }
#ifdef LIBVPSC_LOGGING
        f<<"...remaining blocks="<<bs->size()<<", cost="<<bs->cost()<<endl;
#endif
    }
#ifdef LIBVPSC_LOGGING
    f<<"  finished merges."<<endl;
#endif
    bs->cleanup();
    bool activeConstraints=false;
    for(unsigned i=0;i<m;i++) {
        v=cs[i];
        if(v->active) activeConstraints=true;
        if(v->slack() < ZERO_UPPERBOUND) {
            ostringstream s;
            s<<"Unsatisfied constraint: "<<*v;
#ifdef LIBVPSC_LOGGING
            ofstream f(LOGFILE,ios::app);
            f<<s.str()<<endl;
#endif
            throw s.str().c_str();
        }
    }
#ifdef LIBVPSC_LOGGING
    f<<"  finished cleanup."<<endl;
    printBlocks();
#endif
    copyResult();
    return activeConstraints;
}
void IncSolver::moveBlocks() {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"moveBlocks()..."<<endl;
#endif
    size_t length = bs->size();
    for (size_t i = 0; i < length; ++i)
    {
        Block *b = bs->at(i);
        b->updateWeightedPosition();
        //b->posn = b->wposn / b->weight;
    }
#ifdef LIBVPSC_LOGGING
    f<<"  moved blocks."<<endl;
#endif
}
void IncSolver::splitBlocks() {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
#endif
    moveBlocks();
    splitCnt=0;
    // Split each block if necessary on min LM
    size_t length = bs->size();
    for (size_t i = 0; i < length; ++i)
    {
        Block *b = bs->at(i);
        Constraint* v=b->findMinLM();
        if(v!=nullptr && v->lm < LAGRANGIAN_TOLERANCE) {
            COLA_ASSERT(!v->equality);
#ifdef LIBVPSC_LOGGING
            f<<"    found split point: "<<*v<<" lm="<<v->lm<<endl;
#endif
            splitCnt++;
            Block *b = v->left->block, *l=nullptr, *r=nullptr;
            COLA_ASSERT(v->left->block == v->right->block);
            //double pos = b->posn;
            b->split(l,r,v);
            //l->posn=r->posn=pos;
            //l->wposn = l->posn * l->weight;
            //r->wposn = r->posn * r->weight;
            l->updateWeightedPosition();
            r->updateWeightedPosition();
            bs->insert(l);
            bs->insert(r);
            b->deleted=true;
            COLA_ASSERT(!v->active);
            inactive.push_back(v);
#ifdef LIBVPSC_LOGGING
            f<<"  new blocks: "<<*l<<" and "<<*r<<endl;
#endif
        }
    }
    //if(splitCnt>0) { std::cout<<"  splits: "<<splitCnt<<endl; }
#ifdef LIBVPSC_LOGGING
    f<<"  finished splits."<<endl;
#endif
    bs->cleanup();
}

/*
 * Scan constraint list for the most violated constraint, or the first equality
 * constraint
 */
Constraint* IncSolver::mostViolated(Constraints &l)
{
    double slackForMostViolated = DBL_MAX;
    Constraint* mostViolated = nullptr;
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f << "Looking for most violated..." << endl;
#endif
    size_t lSize = l.size();
    size_t deleteIndex = lSize;
    Constraint *constraint = nullptr;
    double slack = 0;
    for (size_t index = 0; index < lSize; ++index)
    {
        constraint = l[index];
        slack = constraint->slack();
        if (constraint->equality || slack < slackForMostViolated)
        {
            slackForMostViolated = slack;    
            mostViolated = constraint;
            deleteIndex = index;
            if (constraint->equality)
            {
                break;
            }
        }
    }
    // Because the constraint list is not order dependent we just
    // move the last element over the deletePoint and resize
    // downwards.  There is always at least 1 element in the
    // vector because of search.
    if ( (deleteIndex < lSize) && 
         (((slackForMostViolated < ZERO_UPPERBOUND) && !mostViolated->active) || 
          mostViolated->equality) )
    {
        l[deleteIndex] = l[lSize-1];
        l.resize(lSize-1);
    }
#ifdef LIBVPSC_LOGGING
    if (mostViolated)
    {
        f << "  most violated is: " << *mostViolated << endl;
    }
    else
    {
        f << "  non found." << endl;
    }
#endif
    return mostViolated;
}


using std::set;
using std::vector;
using std::iterator;
using std::list;
using std::copy;
#define __NOTNAN(p) (p)==(p)


Blocks::Blocks(vector<Variable*> const &vs) : vs(vs),nvs(vs.size()) {
    blockTimeCtr=0;
    m_blocks.resize(nvs);
    for(size_t i=0;i<nvs;i++) {
        m_blocks[i] = new Block(this, vs[i]);
    }
}
Blocks::~Blocks(void)
{
    blockTimeCtr=0;
    size_t length = m_blocks.size();
    for (size_t i = 0; i < length; ++i)
    {
        delete m_blocks[i];
    }
    m_blocks.clear();
}

/*
 * returns a list of variables with total ordering determined by the constraint 
 * DAG
 */
list<Variable*> *Blocks::totalOrder() {
    list<Variable*> *order = new list<Variable*>;
    for(size_t i=0;i<nvs;i++) {
        vs[i]->visited=false;
    }
    for(size_t i=0;i<nvs;i++) {
        if(vs[i]->in.size()==0) {
            dfsVisit(vs[i],order);
        }
    }
    return order;
}
// Recursive depth first search giving total order by pushing nodes in the DAG
// onto the front of the list when we finish searching them
void Blocks::dfsVisit(Variable *v, list<Variable*> *order) {
    v->visited=true;
    vector<Constraint*>::iterator it=v->out.begin();
    for(;it!=v->out.end();++it) {
        Constraint *c=*it;
        if(!c->right->visited) {
            dfsVisit(c->right, order);
        }
    }    
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  order="<<*v<<endl;
#endif
    order->push_front(v);
}
/*
 * Processes incoming constraints, most violated to least, merging with the
 * neighbouring (left) block until no more violated constraints are found
 */
void Blocks::mergeLeft(Block *r) {    
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"mergeLeft called on "<<*r<<endl;
#endif
    r->timeStamp=++blockTimeCtr;
    r->setUpInConstraints();
    Constraint *c=r->findMinInConstraint();
    while (c != nullptr && c->slack()<0) {
#ifdef LIBVPSC_LOGGING
        f<<"mergeLeft on constraint: "<<*c<<endl;
#endif
        r->deleteMinInConstraint();
        Block *l = c->left->block;        
        if (l->in==nullptr) l->setUpInConstraints();
        double dist = c->right->offset - c->left->offset - c->gap;
        if (r->vars->size() < l->vars->size()) {
            dist=-dist;
            std::swap(l, r);
        }
        blockTimeCtr++;
        r->merge(l, c, dist);
        r->mergeIn(l);
        r->timeStamp=blockTimeCtr;
        removeBlock(l);
        c=r->findMinInConstraint();
    }        
#ifdef LIBVPSC_LOGGING
    f<<"merged "<<*r<<endl;
#endif
}    
/*
 * Symmetrical to mergeLeft
 */
void Blocks::mergeRight(Block *l) {    
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"mergeRight called on "<<*l<<endl;
#endif    
    l->setUpOutConstraints();
    Constraint *c = l->findMinOutConstraint();
    while (c != nullptr && c->slack()<0) {        
#ifdef LIBVPSC_LOGGING
        f<<"mergeRight on constraint: "<<*c<<endl;
#endif
        l->deleteMinOutConstraint();
        Block *r = c->right->block;
        r->setUpOutConstraints();
        double dist = c->left->offset + c->gap - c->right->offset;
        if (l->vars->size() > r->vars->size()) {
            dist=-dist;
            std::swap(l, r);
        }
        l->merge(r, c, dist);
        l->mergeOut(r);
        removeBlock(r);
        c=l->findMinOutConstraint();
    }    
#ifdef LIBVPSC_LOGGING
    f<<"merged "<<*l<<endl;
#endif
}
void Blocks::removeBlock(Block *doomed) {
    doomed->deleted=true;
    //erase(doomed);
}

// Clears up deleted blocks from the blocks list.
void Blocks::cleanup(void)
{
    // We handle removal of items in-place by doing a single linear pass over 
    // the vector. We use two indexes, j to refer to elements we've checked
    // from the original list and i to refer to the current position we are
    // writing in the updated list.
    size_t i = 0;
    
    // For all items in the current blocks list...
    size_t length = m_blocks.size();
    for (size_t j = 0; j < length; )
    {
        if (m_blocks[j]->deleted)
        {
            // The element is deleted, so free it and increment j.
            delete m_blocks[j];
            ++j;
        }
        else
        {
            // The current element is still valid.
            if (j > i)
            {
                // If we've not looking at same element, then copy from j to i.
                m_blocks[i] = m_blocks[j];
            }
            // Increment both indexes.
            ++i;
            ++j;
        }
    }
    m_blocks.resize(i);
}
/*
 * Splits block b across constraint c into two new blocks, l and r (c's left
 * and right sides respectively)
 */
void Blocks::split(Block *b, Block *&l, Block *&r, Constraint *c) {
    b->split(l,r,c);
    m_blocks.push_back(l);
    m_blocks.push_back(r);
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"Split left: "<<*l<<endl;
    f<<"Split right: "<<*r<<endl;
#endif
    r->posn = b->posn;
    //COLA_ASSERT(r->weight!=0);
    //r->wposn = r->posn * r->weight;
    mergeLeft(l);
    // r may have been merged!
    r = c->right->block;
    r->updateWeightedPosition();
    //r->posn = r->wposn / r->weight;
    mergeRight(r);
    removeBlock(b);

    COLA_ASSERT(__NOTNAN(l->posn));
    COLA_ASSERT(__NOTNAN(r->posn));
}
/*
 * returns the cost total squared distance of variables from their desired
 * positions
 */
double Blocks::cost() {
    double c = 0;
    size_t length = m_blocks.size();
    for (size_t i = 0; i < length; ++i)
    {
        c += m_blocks[i]->cost();
    }
    return c;
}

void PositionStats::addVariable(Variable* v) {
    double ai=scale/v->scale;
    double bi=v->offset/v->scale;
    double wi=v->weight;
    AB+=wi*ai*bi;
    AD+=wi*ai*v->desiredPosition;
    A2+=wi*ai*ai;
    /*
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f << "adding v[" << v->id << "], blockscale=" << scale << ", despos=" 
      << v->desiredPosition << ", ai=" << ai << ", bi=" << bi
      << ", AB=" << AB << ", AD=" << AD << ", A2=" << A2;
#endif
*/
}
void Block::addVariable(Variable* v) {
    v->block=this;
    vars->push_back(v);
    if(ps.A2==0) ps.scale=v->scale;
    //weight+= v->weight;
    //wposn += v->weight * (v->desiredPosition - v->offset);
    //posn=wposn/weight;
    ps.addVariable(v);
    posn=(ps.AD - ps.AB) / ps.A2;
    COLA_ASSERT(__NOTNAN(posn));
    /*
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f << ", posn=" << posn << endl;
#endif
*/
}
Block::Block(Blocks *blocks, Variable* const v)
    : vars(new vector<Variable*>)
    , posn(0)
    //, weight(0)
    //, wposn(0)
    , deleted(false)
    , timeStamp(0)
    , in(nullptr)
    , out(nullptr)
    , blocks(blocks)
{
    if(v!=nullptr) {
        v->offset=0;
        addVariable(v);
    }
}

void Block::updateWeightedPosition() {
    //wposn=0;
    ps.AB=ps.AD=ps.A2=0;
    for (Vit v=vars->begin();v!=vars->end();++v) {
        //wposn += ((*v)->desiredPosition - (*v)->offset) * (*v)->weight;
        ps.addVariable(*v);
    }
    posn=(ps.AD - ps.AB) / ps.A2;
    COLA_ASSERT(__NOTNAN(posn));
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f << ", posn=" << posn << endl;
#endif
}
Block::~Block(void)
{
    delete vars;
    delete in;
    delete out;
}
void Block::setUpInConstraints() {
    setUpConstraintHeap(in,true);
}
void Block::setUpOutConstraints() {
    setUpConstraintHeap(out,false);
}
void Block::setUpConstraintHeap(Heap* &h,bool in) {
    delete h;
    h = new Heap();
    for (Vit i=vars->begin();i!=vars->end();++i) {
        Variable *v=*i;
        vector<Constraint*> *cs=in?&(v->in):&(v->out);
        for (Cit j=cs->begin();j!=cs->end();++j) {
            Constraint *c=*j;
            c->timeStamp=blocks->blockTimeCtr;
            if ( ((c->left->block != this) && in) || 
                 ((c->right->block != this) && !in) )
            {
                h->push(c);
            }
        }
    }
}    
Block* Block::merge(Block* b, Constraint* c) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  merging on: "<<*c<<",c->left->offset="<<c->left->offset<<",c->right->offset="<<c->right->offset<<endl;
#endif
    double dist = c->right->offset - c->left->offset - c->gap;
    Block *l=c->left->block;
    Block *r=c->right->block;
    if (l->vars->size() < r->vars->size()) {
        r->merge(l,c,dist);
    } else {
               l->merge(r,c,-dist);
    }
    Block* mergeBlock=b->deleted?this:b;
#ifdef LIBVPSC_LOGGING
    f<<"  merged block="<<*mergeBlock<<endl;
#endif
    return mergeBlock;
}
/*
 * Merges b into this block across c.  Can be either a
 * right merge or a left merge
 * @param b block to merge into this
 * @param c constraint being merged
 * @param distance separation required to satisfy c
 */
void Block::merge(Block *b, Constraint *c, double dist) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"    merging: "<<*b<<"dist="<<dist<<endl;
#endif
    c->active=true;
    //wposn+=b->wposn-dist*b->weight;
    //weight+=b->weight;
    for(Vit i=b->vars->begin();i!=b->vars->end();++i) {
        Variable *v=*i;
        //v->block=this;
        //vars->push_back(v);
        v->offset+=dist;
        addVariable(v);
    }
#ifdef LIBVPSC_LOGGING
    for(Vit i=vars->begin();i!=vars->end();++i) {
        Variable *v=*i;
        f<<"    v["<<v->id<<"]: d="<<v->desiredPosition
            <<" a="<<v->scale<<" o="<<v->offset
            <<endl;
    }
    f<<"  AD="<<ps.AD<<" AB="<<ps.AB<<" A2="<<ps.A2<<endl;
#endif
    //posn=wposn/weight;
    //COLA_ASSERT(wposn==ps.AD - ps.AB);
    posn=(ps.AD - ps.AB) / ps.A2;
    COLA_ASSERT(__NOTNAN(posn));
    b->deleted=true;
}

void Block::mergeIn(Block *b) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  merging constraint heaps... "<<endl;
#endif
    // We check the top of the heaps to remove possible internal constraints
    findMinInConstraint();
    b->findMinInConstraint();
    while (!b->in->empty())
    {
        in->push(b->in->top());
        b->in->pop();
    }
#ifdef LIBVPSC_LOGGING
    f<<"  merged heap: "<<*in<<endl;
#endif
}
void Block::mergeOut(Block *b) {    
    findMinOutConstraint();
    b->findMinOutConstraint();
    while (!b->out->empty())
    {
        out->push(b->out->top());
        b->out->pop();
    }
}
Constraint *Block::findMinInConstraint() {
    Constraint *v = nullptr;
    vector<Constraint*> outOfDate;
    while (!in->empty()) {
        v = in->top();
        Block *lb=v->left->block;
        Block *rb=v->right->block;
        // rb may not be this if called between merge and mergeIn
#ifdef LIBVPSC_LOGGING
        ofstream f(LOGFILE,ios::app);
        f<<"  checking constraint ... "<<*v;
        f<<"    timestamps: left="<<lb->timeStamp<<" right="<<rb->timeStamp<<" constraint="<<v->timeStamp<<endl;
#endif
        if(lb == rb) {
            // constraint has been merged into the same block
#ifdef LIBVPSC_LOGGING
            if(v->slack()<0) {
                f<<"  violated internal constraint found! "<<*v<<endl;
                f<<"     lb="<<*lb<<endl;
                f<<"     rb="<<*rb<<endl;
            }
#endif
            in->pop();
#ifdef LIBVPSC_LOGGING
            f<<" ... skipping internal constraint"<<endl;
#endif
        } else if(v->timeStamp < lb->timeStamp) {
            // block at other end of constraint has been moved since this
            in->pop();
            outOfDate.push_back(v);
#ifdef LIBVPSC_LOGGING
            f<<"    reinserting out of date (reinsert later)"<<endl;
#endif
        } else {
            break;
        }
    }
    for(Cit i=outOfDate.begin();i!=outOfDate.end();++i) {
        v=*i;
        v->timeStamp=blocks->blockTimeCtr;
        in->push(v);
    }
    if(in->empty()) {
        v=nullptr;
    } else {
        v=in->top();
    }
    return v;
}
Constraint *Block::findMinOutConstraint() {
    if(out->empty()) return nullptr;
    Constraint *v = out->top();
    while (v->left->block == v->right->block) {
        out->pop();
        if(out->empty()) return nullptr;
        v = out->top();
    }
    return v;
}
void Block::deleteMinInConstraint() {
    in->pop();
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"deleteMinInConstraint... "<<endl;
    f<<"  result: "<<*in<<endl;
#endif
}
void Block::deleteMinOutConstraint() {
    out->pop();
}
inline bool Block::canFollowLeft(Constraint const* c, Variable const* last) const {
    return c->left->block==this && c->active && last!=c->left;
}
inline bool Block::canFollowRight(Constraint const* c, Variable const* last) const {
    return c->right->block==this && c->active && last!=c->right;
}

// computes the derivative of v and the lagrange multipliers
// of v's out constraints (as the recursive sum of those below.
// Does not backtrack over u.
// also records the constraint with minimum lagrange multiplier
// in min_lm
double Block::compute_dfdv(Variable* const v, Variable* const u,
               Constraint *&min_lm) {
    double dfdv=v->dfdv();
    for(Cit it=v->out.begin();it!=v->out.end();++it) {
        Constraint *c=*it;
        if(canFollowRight(c,u)) {
            c->lm=compute_dfdv(c->right,v,min_lm);
            dfdv+=c->lm*c->left->scale;
            if(!c->equality&&(min_lm==nullptr||c->lm<min_lm->lm)) min_lm=c;
        }
    }
    for(Cit it=v->in.begin();it!=v->in.end();++it) {
        Constraint *c=*it;
        if(canFollowLeft(c,u)) {
            c->lm=-compute_dfdv(c->left,v,min_lm);
            dfdv-=c->lm*c->right->scale;
            if(!c->equality&&(min_lm==nullptr||c->lm<min_lm->lm)) min_lm=c;
        }
    }
    return dfdv/v->scale;
}
double Block::compute_dfdv(Variable* const v, Variable* const u) {
    double dfdv = v->dfdv();
    for(Cit it = v->out.begin(); it != v->out.end(); ++it) {
        Constraint *c = *it;
        if(canFollowRight(c,u)) {
            c->lm =   compute_dfdv(c->right,v);
            dfdv += c->lm * c->left->scale;
        }
    }
    for(Cit it=v->in.begin();it!=v->in.end();++it) {
        Constraint *c = *it;
        if(canFollowLeft(c,u)) {
            c->lm = - compute_dfdv(c->left,v);
            dfdv -= c->lm * c->right->scale;
        }
    }
    return dfdv/v->scale;
}

// The top level v and r are variables between which we want to find the
// constraint with the smallest lm.  
// Similarly, m is initially nullptr and is only assigned a value if the next
// variable to be visited is r or if a possible min constraint is returned from
// a nested call (rather than nullptr).
// Then, the search for the m with minimum lm occurs as we return from
// the recursion (checking only constraints traversed left-to-right 
// in order to avoid creating any new violations).
// We also do not consider equality constraints as potential split points
bool Block::split_path(
    Variable* r, 
    Variable* const v, 
    Variable* const u, 
    Constraint* &m,
    bool desperation=false
    ) 
{
    for(Cit it(v->in.begin());it!=v->in.end();++it) {
        Constraint *c=*it;
        if(canFollowLeft(c,u)) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  left split path: "<<*c<<endl;
#endif
            if(c->left==r) {
                if(desperation&&!c->equality) m=c;
                return true;
            } else {
                if(split_path(r,c->left,v,m)) {
                    if(desperation && !c->equality && (!m||c->lm<m->lm)) {
                               m=c;
                    }
                    return true;
                }
            }
        }
    }
    for(Cit it(v->out.begin());it!=v->out.end();++it) {
        Constraint *c=*it;
        if(canFollowRight(c,u)) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  right split path: "<<*c<<endl;
#endif
            if(c->right==r) {
                if(!c->equality) m=c;
                return true;
            } else {
                if(split_path(r,c->right,v,m)) {
                    if(!c->equality && (!m||c->lm<m->lm))
                               m=c;
                    return true;
                }
            }
        }
    }
    return false;
}
/*
Block::Pair Block::compute_dfdv_between(
        Variable* r, Variable* const v, Variable* const u, 
        const Direction dir = NONE, bool changedDirection = false) {
    double dfdv=v->weight*(v->position() - v->desiredPosition);
    Constraint *m=nullptr;
    for(Cit it(v->in.begin());it!=v->in.end();++it) {
        Constraint *c=*it;
        if(canFollowLeft(c,u)) {
            if(dir==RIGHT) { 
                changedDirection = true; 
            }
            if(c->left==r) {
                       r=nullptr;
                    if(!c->equality) m=c; 
            }
            Pair p=compute_dfdv_between(r,c->left,v,
                    LEFT,changedDirection);
            dfdv -= c->lm = -p.first;
            if(r && p.second) 
                m = p.second;
        }
    }
    for(Cit it(v->out.begin());it!=v->out.end();++it) {
        Constraint *c=*it;
        if(canFollowRight(c,u)) {
            if(dir==LEFT) { 
                changedDirection = true; 
            }
            if(c->right==r) {
                       r=nullptr; 
                    if(!c->equality) m=c; 
            }
            Pair p=compute_dfdv_between(r,c->right,v,
                    RIGHT,changedDirection);
            dfdv += c->lm = p.first;
            if(r && p.second) 
                m = changedDirection && !c->equality && c->lm < p.second->lm 
                    ? c 
                    : p.second;
        }
    }
    return Pair(dfdv,m);
}
*/

// resets LMs for all active constraints to 0 by
// traversing active constraint tree starting from v,
// not back tracking over u
void Block::reset_active_lm(Variable* const v, Variable* const u) {
    for(Cit it=v->out.begin();it!=v->out.end();++it) {
        Constraint *c=*it;
        if(canFollowRight(c,u)) {
            c->lm=0;
            reset_active_lm(c->right,v);
        }
    }
    for(Cit it=v->in.begin();it!=v->in.end();++it) {
        Constraint *c=*it;
        if(canFollowLeft(c,u)) {
            c->lm=0;
            reset_active_lm(c->left,v);
        }
    }
}
void Block::list_active(Variable* const v, Variable* const u) {
    for(Cit it=v->out.begin();it!=v->out.end();++it) {
        Constraint *c=*it;
        if(canFollowRight(c,u)) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  "<<*c<<endl;
#endif
            list_active(c->right,v);
        }
    }
    for(Cit it=v->in.begin();it!=v->in.end();++it) {
        Constraint *c=*it;
        if(canFollowLeft(c,u)) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  "<<*c<<endl;
#endif
            list_active(c->left,v);
        }
    }
}
/*
 * finds the constraint with the minimum lagrange multiplier, that is, the constraint
 * that most wants to split
 */
Constraint *Block::findMinLM() {
    Constraint *min_lm=nullptr;
    reset_active_lm(vars->front(),nullptr);
    compute_dfdv(vars->front(),nullptr,min_lm);
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  langrangians: "<<endl;
    list_active(vars->front(),nullptr);
#endif
    return min_lm;
}
Constraint *Block::findMinLMBetween(Variable* const lv, Variable* const rv) {
    reset_active_lm(vars->front(),nullptr);
    compute_dfdv(vars->front(),nullptr);
    Constraint *min_lm=nullptr;
    split_path(rv,lv,nullptr,min_lm);
#if 0
    if(min_lm==nullptr) {
        split_path(rv,lv,nullptr,min_lm,true);
    }
#else
    if(min_lm==nullptr) {
        //err_printf("Couldn't find split point!\n");
        UnsatisfiableException e;
        getActivePathBetween(e.path,lv,rv,nullptr);
        throw e;
    }
    COLA_ASSERT(min_lm!=nullptr);
#endif
    return min_lm;
}

// populates block b by traversing the active constraint tree adding variables as they're 
// visited.  Starts from variable v and does not backtrack over variable u.
void Block::populateSplitBlock(Block *b, Variable* v, Variable const* u) {
    b->addVariable(v);
    for (Cit c=v->in.begin();c!=v->in.end();++c) {
        if (canFollowLeft(*c,u))
            populateSplitBlock(b, (*c)->left, v);
    }
    for (Cit c=v->out.begin();c!=v->out.end();++c) {
        if (canFollowRight(*c,u)) 
            populateSplitBlock(b, (*c)->right, v);
    }
}
/*
 * Returns the active path between variables u and v... not back tracking over w
 */
bool Block::getActivePathBetween(Constraints& path, Variable const* u,
               Variable const* v, Variable const *w) const {
    if(u==v) return true;
    for (Cit_const c=u->in.begin();c!=u->in.end();++c) {
        if (canFollowLeft(*c,w)) {
            if(getActivePathBetween(path, (*c)->left, v, u)) {
                path.push_back(*c);
                return true;
            }
        }
    }
    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
        if (canFollowRight(*c,w)) {
            if(getActivePathBetween(path, (*c)->right, v, u)) {
                path.push_back(*c);
                return true;
            }
        }
    }
    return false;
}
// Search active constraint tree from u to see if there is a directed path to v.
// Returns true if path is found with all constraints in path having their visited flag
// set true.
bool Block::isActiveDirectedPathBetween(Variable const* u, Variable const* v) const {
    if(u==v) return true;
    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
        if(canFollowRight(*c,nullptr)) {
            if(isActiveDirectedPathBetween((*c)->right,v)) {
                return true;
            }
        }
    }
    return false;
}
bool Block::getActiveDirectedPathBetween(
        Constraints& path, Variable const* u, Variable const* v) const {
    if(u==v) return true;
    for (Cit_const c=u->out.begin();c!=u->out.end();++c) {
        if(canFollowRight(*c,nullptr)) {
            if(getActiveDirectedPathBetween(path,(*c)->right,v)) {
                path.push_back(*c);
                return true;
            }
        }
    }
    return false;
}
/*
 * Block needs to be split because of a violated constraint between vl and vr.
 * We need to search the active constraint tree between l and r and find the constraint
 * with min lagrangrian multiplier and split at that point.
 * Returns the split constraint
 */
Constraint* Block::splitBetween(Variable* const vl, Variable* const vr,
               Block* &lb, Block* &rb) {
#ifdef LIBVPSC_LOGGING
    ofstream f(LOGFILE,ios::app);
    f<<"  need to split between: "<<*vl<<" and "<<*vr<<endl;
#endif
    Constraint *c=findMinLMBetween(vl, vr);
#ifdef LIBVPSC_LOGGING
    f<<"  going to split on: "<<*c<<endl;
#endif
    if(c!=nullptr) {
        split(lb,rb,c);
        deleted = true;
    }
    return c;
}

/*
 * Creates two new blocks, l and r, and splits this block across constraint c,
 * placing the left subtree of constraints (and associated variables) into l
 * and the right into r.
 */
void Block::split(Block* &l, Block* &r, Constraint* c) {
    c->active=false;
    l=new Block(blocks);
    populateSplitBlock(l,c->left,c->right);
    //COLA_ASSERT(l->weight>0);
    r=new Block(blocks);
    populateSplitBlock(r,c->right,c->left);
    //COLA_ASSERT(r->weight>0);
}

/*
 * Computes the cost (squared euclidean distance from desired positions) of the
 * current positions for variables in this block
 */
double Block::cost() {
    double c = 0;
    for (Vit v=vars->begin();v!=vars->end();++v) {
        double diff = (*v)->position() - (*v)->desiredPosition;
        c += (*v)->weight * diff * diff;
    }
    return c;
}
ostream& operator <<(ostream &os, const Block& b)
{
    os<<"Block(posn="<<b.posn<<"):";
    for(Block::Vit v=b.vars->begin();v!=b.vars->end();++v) {
        os<<" "<<**v;
    }
    if(b.deleted) {
        os<<" Deleted!";
    }
    return os;
}

Constraint::Constraint(Variable *left, Variable *right, double gap, bool equality)
: left(left),
  right(right),
  gap(gap),
  timeStamp(0),
  active(false),
  equality(equality),
  unsatisfiable(false),
  needsScaling(true),
  creator(nullptr)
{
    // In hindsight I think it's probably better to build the constraint DAG
    // (by creating variable in/out lists) when needed, rather than in advance
    //left->out.push_back(this);
    //right->in.push_back(this);
}
Constraint::~Constraint() {
    // see constructor: the following is just way too slow.  
    // Better to create a
    // new DAG on demand than maintain the lists dynamically.
    //Constraints::iterator i;
    //for(i=left->out.begin(); i!=left->out.end(); i++) {
        //if(*i==this) break;
    //}
    //left->out.erase(i);
    //for(i=right->in.begin(); i!=right->in.end(); i++) {
        //if(*i==this) break;
    //}
    //right->in.erase(i);
}
std::string Constraint::toString(void) const
{
    std::stringstream stream;
    stream << "Constraint: var(" << left->id << ") ";
    if (gap < 0)
    {
        stream << "- " << -gap << " ";
    }
    else
    {
        stream << "+ " << gap << " ";
    }
    stream << ((equality) ? "==" : "<=");
    stream << " var(" << right->id << ") ";
    return stream.str();
}

std::ostream& operator <<(std::ostream &os, const Constraint &c)
{
    const char *type = c.equality ? "=" : "<=";
    std::ostringstream lscale, rscale;
    if (c.left->scale != 1) 
    {
        lscale << c.left->scale << "*";
    }
    if (c.right->scale != 1)
    {
        rscale << c.right->scale << "*";
    }
    os << lscale.str() << *c.left << "+" << c.gap << type << 
          rscale.str() << *c.right;
    if (c.left->block && c.right->block)
    {
        os << "(" << c.slack() << ")" << (c.active ? "-active" : "") <<
              "(lm=" << c.lm << ")";
    }
    else
    {
        os << "(vars have no position)";
    }
    return os;
}

bool CompareConstraints::operator() (
    Constraint *const &l, Constraint *const &r
) const {
    double const sl = 
        l->left->block->timeStamp > l->timeStamp
        ||l->left->block==l->right->block
        ?-DBL_MAX:l->slack();
    double const sr = 
        r->left->block->timeStamp > r->timeStamp
        ||r->left->block==r->right->block
        ?-DBL_MAX:r->slack();
    if(sl==sr) {
        // arbitrary choice based on id
        if(l->left->id==r->left->id) {
            if(l->right->id<r->right->id) return true;
            return false;
        }
        if(l->left->id<r->left->id) return true;
        return false;
    }
    return sl > sr;
}

std::ostream& operator <<(std::ostream &os, const Variable &v) {
    if(v.block)
        os << "(" << v.id << "=" << v.position() << ")";
    else
        os << "(" << v.id << "=" << v.desiredPosition << ")";
    return os;
}

typedef std::list<std::map<Variable *, double> > VarOffsetMapList;

class EqualityConstraintSet
{
    public:
        EqualityConstraintSet(Variables vs)
        {
            for (size_t i = 0; i < vs.size(); ++i)
            {
                std::map<Variable *, double> varSet;
                varSet[vs[i]] = 0;
                variableGroups.push_back(varSet);
            }
        }
        bool isRedundant(Variable *lhs, Variable *rhs, double sep)
        {
            VarOffsetMapList::iterator lhsSet = setForVar(lhs);
            VarOffsetMapList::iterator rhsSet = setForVar(rhs);
            if (lhsSet == rhsSet)
            {
                // Check if this is a redundant constraint.
                if (fabs(((*lhsSet)[lhs] + sep) - (*rhsSet)[rhs]) < 0.0001)
                {
                    // If so, return true.
                    return true;
                }
            }
            return false;
        }
        void mergeSets(Variable *lhs, Variable *rhs, double sep)
        {
            VarOffsetMapList::iterator lhsSet = setForVar(lhs);
            VarOffsetMapList::iterator rhsSet = setForVar(rhs);
            if (lhsSet == rhsSet)
            {
                return;
            }

            double rhsOldOffset = (*rhsSet)[rhs];
            double rhsNewOffset = (*lhsSet)[lhs] + sep;
            double offset = rhsNewOffset - rhsOldOffset;

            // Update offsets
            for (std::map<Variable *, double>::iterator it = rhsSet->begin();
                    it != rhsSet->end(); ++it)
            {
                it->second += offset;
            }
            
            // Merge rhsSet into lhsSet
            lhsSet->insert(rhsSet->begin(), rhsSet->end());
            variableGroups.erase(rhsSet);
        }

    private:
        VarOffsetMapList::iterator setForVar(Variable *var)
        {
            for (VarOffsetMapList::iterator it = variableGroups.begin();
                    it != variableGroups.end(); ++it)
            {
                if (it->find(var) != it->end())
                {
                    return it;
                }
            }
            return variableGroups.end();
        }

    VarOffsetMapList variableGroups;
};

Constraints constraintsRemovingRedundantEqualities(Variables const &vars, 
        Constraints const &constraints)
{
    EqualityConstraintSet equalitySets(vars);
    Constraints cs = Constraints(constraints.size());
    int csSize = 0;

    for (unsigned i = 0; i < constraints.size(); ++i)
    {
        Constraint *c = constraints[i];
        if (c->equality)
        {
            if (!equalitySets.isRedundant(c->left, c->right, c->gap))
            {
                // Only add non-redundant equalities
                equalitySets.mergeSets(c->left, c->right, c->gap);
                cs[csSize++] = c;
            }
        }
        else
        {
            // Add all non-equalities
            cs[csSize++] = c;
        }
    }
    cs.resize(csSize);
    return cs;
}


}

#endif