/**
 * \brief A block is a group of variables that must be moved together to improve
 * the goal function without violating already active constraints.
 * The variables in a block are spanned by a tree of active constraints.
 *
 * Authors:
 *   Tim Dwyer <tgdwyer@gmail.com>
 *
 * Copyright (C) 2005 Authors
 *
 * Released under GNU LGPL.  Read the file 'COPYING' for more information.
 */
#include <cassert>
#include "PairingHeap.h"
#include "constraint.h"
#include "block.h"
#include "blocks.h"
#ifdef RECTANGLE_OVERLAP_LOGGING
#include <fstream>
using std::endl;
using std::ios;
using std::ofstream;
#endif
using std::vector;

namespace vpsc {
void Block::addVariable(Variable *const v) {
  v->block = this;
  vars->push_back(v);
  weight += v->weight;
  wposn += v->weight * (v->desiredPosition - v->offset);
  posn = wposn / weight;
}
Block::Block(Variable *const v) {
  timeStamp = 0;
  posn = weight = wposn = 0;
  in = nullptr;
  out = nullptr;
  deleted = false;
  vars = new vector<Variable *>;

  if (v != nullptr) {
    v->offset = 0;
    addVariable(v);
  }
}

double Block::desiredWeightedPosition() {
  double wp = 0;

  for (Vit v = vars->begin(); v != vars->end(); ++v) {
    wp += ((*v)->desiredPosition - (*v)->offset) * (*v)->weight;
  }

  return wp;
}
Block::~Block(void) {
  delete vars;
  delete in;
  delete out;
}
void Block::setUpInConstraints() {
  setUpConstraintHeap(in, true);
}
void Block::setUpOutConstraints() {
  setUpConstraintHeap(out, false);
}
void Block::setUpConstraintHeap(PairingHeap<Constraint *> *&h, bool in) {
  delete h;
  h = new PairingHeap<Constraint *>(&compareConstraints);

  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 = blockTimeCtr;

      if ((c->left->block != this && in) || (c->right->block != this && !in)) {
        h->insert(c);
      }
    }
  }
}
void Block::merge(Block *b, Constraint *c) {
#ifdef RECTANGLE_OVERLAP_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 (vars->size() < b->vars->size()) {
    r->merge(l, c, dist);
  } else {
    l->merge(r, c, -dist);
  }

#ifdef RECTANGLE_OVERLAP_LOGGING
  f << "  merged block=" << (b->deleted ? *this : *b) << endl;
#endif
}
/**
 * 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 RECTANGLE_OVERLAP_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;
  posn = wposn / weight;

  for (Vit i = b->vars->begin(); i != b->vars->end(); ++i) {
    Variable *v = *i;
    v->block = this;
    v->offset += dist;
    vars->push_back(v);
  }

  b->deleted = true;
}

void Block::mergeIn(Block *b) {
#ifdef RECTANGLE_OVERLAP_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();
  in->merge(b->in);
#ifdef RECTANGLE_OVERLAP_LOGGING
  f << "  merged heap: " << *in << endl;
#endif
}
void Block::mergeOut(Block *b) {
  findMinOutConstraint();
  b->findMinOutConstraint();
  out->merge(b->out);
}
Constraint *Block::findMinInConstraint() {
  Constraint *v = nullptr;
  vector<Constraint *> outOfDate;

  while (!in->isEmpty()) {
    v = in->findMin();
    Block *lb = v->left->block;
    Block *rb = v->right->block;
// rb may not be this if called between merge and mergeIn
#ifdef RECTANGLE_OVERLAP_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 RECTANGLE_OVERLAP_LOGGING
      if (v->slack() < 0) {
        f << "  violated internal constraint found! " << *v << endl;
        f << "     lb=" << *lb << endl;
        f << "     rb=" << *rb << endl;
      }

#endif
      in->deleteMin();
#ifdef RECTANGLE_OVERLAP_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->deleteMin();
      outOfDate.push_back(v);
#ifdef RECTANGLE_OVERLAP_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 = blockTimeCtr;
    in->insert(v);
  }

  if (in->isEmpty()) {
    v = nullptr;
  } else {
    v = in->findMin();
  }

  return v;
}
Constraint *Block::findMinOutConstraint() {
  if (out->isEmpty())
    return nullptr;

  Constraint *v = out->findMin();

  while (v->left->block == v->right->block) {
    out->deleteMin();

    if (out->isEmpty())
      return nullptr;

    v = out->findMin();
  }

  return v;
}
void Block::deleteMinInConstraint() {
  in->deleteMin();
#ifdef RECTANGLE_OVERLAP_LOGGING
  ofstream f(LOGFILE, ios::app);
  f << "deleteMinInConstraint... " << endl;
  f << "  result: " << *in << endl;
#endif
}
void Block::deleteMinOutConstraint() {
  out->deleteMin();
}
inline bool Block::canFollowLeft(Constraint *c, const Variable *const last) {
  return c->left->block == this && c->active && last != c->left;
}
inline bool Block::canFollowRight(Constraint *c, const Variable *const last) {
  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->weight * (v->position() - v->desiredPosition);

  for (Cit it = v->out.begin(); it != v->out.end(); ++it) {
    Constraint *c = *it;

    if (canFollowRight(c, u)) {
      dfdv += c->lm = compute_dfdv(c->right, v, min_lm);

      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)) {
      dfdv -= c->lm = -compute_dfdv(c->left, v, min_lm);

      if (!c->equality && (min_lm == nullptr || c->lm < min_lm->lm))
        min_lm = c;
    }
  }

  return dfdv;
}

// computes dfdv for each variable and uses the sum of dfdv on either side of
// the constraint c to compute the lagrangian multiplier lm_c.
// The top level v and r are variables between which we want to find the
// constraint with the smallest lm.
// When we find r we pass nullptr to subsequent recursive calls,
// thus r=nullptr indicates constraints are not on the shortest path.
// 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
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);
    }
  }
}
/**
 * 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);
  return min_lm;
}
Constraint *Block::findMinLMBetween(Variable *const lv, Variable *const rv) {
  reset_active_lm(vars->front(), nullptr);
  Constraint *min_lm = compute_dfdv_between(rv, lv, nullptr).second;
  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 *const 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);
  }
}
// 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 *u, Variable *v) {
  if (u == v)
    return true;

  for (Cit c = u->out.begin(); c != u->out.end(); ++c) {
    if (canFollowRight(*c, nullptr)) {
      if (isActiveDirectedPathBetween((*c)->right, v)) {
        (*c)->visited = true;
        return true;
      }

      (*c)->visited = false;
    }
  }

  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 RECTANGLE_OVERLAP_LOGGING
  ofstream f(LOGFILE, ios::app);
  f << "  need to split between: " << *vl << " and " << *vr << endl;
#endif
  Constraint *c = findMinLMBetween(vl, vr);
#ifdef RECTANGLE_OVERLAP_LOGGING
  f << "  going to split on: " << *c << endl;
#endif
  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();
  populateSplitBlock(l, c->left, c->right);
  r = new Block();
  populateSplitBlock(r, c->right, c->left);
}

/**
 * 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:";

  for (Block::Vit v = b.vars->begin(); v != b.vars->end(); ++v) {
    os << " " << **v;
  }

  if (b.deleted) {
    os << " Deleted!";
  }

  return os;
}
} // namespace vpsc
