#include "decaf.h"

#include <boost/range/combine.hpp>
#include <boost/range/iterator_range.hpp>
#include <fmt/format.h>
#include <llvm/ADT/SmallString.h>
#include <llvm/IR/Instructions.h>
#include <llvm/Support/raw_ostream.h>

#include <iostream>
#include <optional>
#include <stdexcept>

namespace decaf {
/************************************************
 * Executor                                     *
 ************************************************/
void Executor::add_context(Context &&ctx) {
  contexts.push_back(ctx);
}

Context Executor::next_context() {
  DECAF_ASSERT(has_next());

  Context ctx = std::move(contexts.back());
  contexts.pop_back();
  return ctx;
}

bool Executor::has_next() const {
  return !contexts.empty();
}

/************************************************
 * StackFrame                                   *
 ************************************************/
StackFrame::StackFrame(llvm::Function *function)
    : function(function), current_block(&function->getEntryBlock()), prev_block(nullptr),
      current(current_block->begin()) {}

void StackFrame::jump_to(llvm::BasicBlock *block) {
  prev_block = current_block;
  current_block = block;
  current = block->begin();
}

void StackFrame::insert(llvm::Value *value, const z3::expr &expr) {
  variables.insert_or_assign(value, expr);
}

z3::expr StackFrame::lookup(llvm::Value *value, z3::context &ctx) const {
  if (auto *constant = llvm::dyn_cast_or_null<llvm::Constant>(value))
    return evaluate_constant(ctx, constant);

  auto it = variables.find(value);
  DECAF_ASSERT(it != variables.end(), "Tried to access unknown variable");
  return it->second;
}

/************************************************
 * Context                                      *
 ************************************************/

Context::Context(z3::context &z3, llvm::Function *function)
    : solver(z3::tactic(z3, "default").mk_solver()) {
  stack.emplace_back(function);
  StackFrame &frame = stack_top();

  int argnum = 0;
  for (auto &arg : function->args()) {
    z3::sort sort = sort_for_type(z3, arg.getType());
    z3::symbol symbol = z3.int_symbol(argnum);

    frame.variables.insert({&arg, z3.constant(symbol, sort)});
    argnum += 1;
  }
}
Context::Context(const Context &ctx, z3::solver &&solver)
    : stack(ctx.stack), solver(std::move(solver)) {}

StackFrame &Context::stack_top() {
  DECAF_ASSERT(!stack.empty());
  return stack.back();
}

z3::check_result Context::check(const z3::expr &expr) {
  auto cond = normalize_to_bool(expr);
  DECAF_ASSERT(cond.is_bool());
  return solver.check(1, &cond);
}
z3::check_result Context::check() {
  return solver.check();
}

void Context::add(const z3::expr &assertion) {
  DECAF_ASSERT(assertion.is_bool(), "assertions must be booleans");
  solver.add(assertion);
}

Context Context::fork() const {
  z3::solver new_solver = z3::tactic(solver.ctx(), "default").mk_solver();

  for (const auto &assertion : solver.assertions()) {
    new_solver.add(assertion);
  }

  return Context(*this, std::move(new_solver));
}

/************************************************
 * Interpreter                                  *
 ************************************************/
Interpreter::Interpreter(Context *ctx, Executor *queue, z3::context *z3, FailureTracker *tracker)
    : ctx(ctx), queue(queue), z3(z3), tracker(tracker) {}

void Interpreter::execute() {
  ExecutionResult exec;

  do {
    StackFrame &frame = ctx->stack_top();

    DECAF_ASSERT(frame.current != frame.current_block->end(),
                 "Instruction pointer ran off end of block.");

    llvm::Instruction &inst = *frame.current;

    // Note: Need to increment the iterator before actually doing
    //       anything with the instruction since instructions can
    //       modify the current position (e.g. branch, call, etc.)
    ++frame.current;

    exec = visit(inst);
  } while (exec == ExecutionResult::Continue);
}

ExecutionResult Interpreter::visitAdd(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, lhs + rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitSub(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, lhs - rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitMul(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, lhs * rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitICmpInst(llvm::ICmpInst &icmp) {
  using llvm::ICmpInst;

  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(icmp.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(icmp.getOperand(1), *z3));

  switch (icmp.getPredicate()) {
  case ICmpInst::ICMP_EQ:
    frame.insert(&icmp, lhs == rhs);
    break;
  case ICmpInst::ICMP_NE:
    frame.insert(&icmp, lhs != rhs);
    break;
  case ICmpInst::ICMP_UGT:
    frame.insert(&icmp, z3::ugt(lhs, rhs));
    break;
  case ICmpInst::ICMP_UGE:
    frame.insert(&icmp, z3::uge(lhs, rhs));
    break;
  case ICmpInst::ICMP_ULT:
    frame.insert(&icmp, z3::ult(lhs, rhs));
    break;
  case ICmpInst::ICMP_ULE:
    frame.insert(&icmp, z3::ule(lhs, rhs));
    break;
  case ICmpInst::ICMP_SGT:
    frame.insert(&icmp, lhs > rhs);
    break;
  case ICmpInst::ICMP_SGE:
    frame.insert(&icmp, lhs >= rhs);
    break;
  case ICmpInst::ICMP_SLT:
    frame.insert(&icmp, lhs < rhs);
    break;
  case ICmpInst::ICMP_SLE:
    frame.insert(&icmp, lhs <= rhs);
    break;
  default:
    DECAF_UNREACHABLE();
  }

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitSDiv(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  if (ctx->check(rhs == 0 || !z3::bvsdiv_no_overflow(lhs, rhs)) == z3::sat) {
    tracker->add_failure(*ctx, ctx->solver.get_model());
  }
  ctx->add(rhs != 0);
  ctx->add(z3::bvsdiv_no_overflow(lhs, rhs));

  frame.insert(&op, lhs / rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitUDiv(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  if (ctx->check(rhs == 0) == z3::sat) {
    tracker->add_failure(*ctx, ctx->solver.get_model());
  }
  ctx->add(rhs != 0);

  frame.insert(&op, z3::udiv(lhs, rhs));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitSRem(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  if (ctx->check(rhs == 0 || !z3::bvsdiv_no_overflow(lhs, rhs)) == z3::sat) {
    tracker->add_failure(*ctx, ctx->solver.get_model());
  }
  ctx->add(rhs != 0);
  ctx->add(z3::bvsdiv_no_overflow(lhs, rhs));

  frame.insert(&op, lhs % rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitURem(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  if (ctx->check(rhs == 0) == z3::sat) {
    tracker->add_failure(*ctx, ctx->solver.get_model());
  }
  ctx->add(rhs != 0);

  frame.insert(&op, z3::urem(lhs, rhs));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitShl(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, z3::shl(lhs, rhs));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitAShr(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, z3::ashr(lhs, rhs));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitLShr(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, z3::lshr(lhs, rhs));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitAnd(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, lhs & rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitOr(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, lhs | rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitXor(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto lhs = normalize_to_int(frame.lookup(op.getOperand(0), *z3));
  auto rhs = normalize_to_int(frame.lookup(op.getOperand(1), *z3));

  frame.insert(&op, lhs ^ rhs);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitNot(llvm::BinaryOperator &op) {
  StackFrame &frame = ctx->stack_top();

  auto expr = normalize_to_int(frame.lookup(op.getOperand(0), *z3));

  frame.insert(&op, ~expr);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitTrunc(llvm::TruncInst &trunc) {
  auto &frame = ctx->stack_top();

  auto src = normalize_to_int(frame.lookup(trunc.getOperand(0), *z3));
  auto dst_ty = trunc.getDestTy();

  DECAF_ASSERT(dst_ty->isIntegerTy());
  DECAF_ASSERT(dst_ty->getIntegerBitWidth() <= src.get_sort().bv_size());

  frame.insert(&trunc, src.extract(dst_ty->getIntegerBitWidth() - 1, 0));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitBranchInst(llvm::BranchInst &inst) {
  if (!inst.isConditional()) {
    ctx->stack_top().jump_to(inst.getSuccessor(0));
    return ExecutionResult::Continue;
  }

  auto &frame = ctx->stack_top();
  auto cond = normalize_to_bool(frame.lookup(inst.getCondition(), *z3));

  auto is_t = ctx->check(cond);
  auto is_f = ctx->check(!cond);

  // Note: For the purposes of branching we consider unknown to be
  //       equivalent to sat. Maybe future branches will bring the
  //       equation back to being solvable.
  if (is_t != z3::unsat && is_f != z3::unsat) {
    auto fork = ctx->fork();

    // In cases where both conditions are possible we follow the
    // false path. This should be enough to get us out of most loops
    // and actually exploring the rest of the program.
    fork.add(cond);
    ctx->add(!cond);

    fork.stack_top().jump_to(inst.getSuccessor(0));
    ctx->stack_top().jump_to(inst.getSuccessor(1));

    queue->add_context(std::move(fork));
    return ExecutionResult::Continue;
  } else if (is_t != z3::unsat) {
    ctx->add(cond);
    ctx->stack_top().jump_to(inst.getSuccessor(0));
    return ExecutionResult::Continue;
  } else if (is_f != z3::unsat) {
    ctx->add(!cond);
    ctx->stack_top().jump_to(inst.getSuccessor(1));
    return ExecutionResult::Continue;
  } else {
    return ExecutionResult::Stop;
  }
}

ExecutionResult Interpreter::visitReturnInst(llvm::ReturnInst &inst) {
  auto &frame = ctx->stack_top();

  std::optional<z3::expr> result;
  if (inst.getNumOperands() != 0)
    result = frame.lookup(inst.getOperand(0), *z3);

  ctx->stack.pop_back();

  // Reached end of function, nothing left to do.
  if (ctx->stack.empty())
    return ExecutionResult::Stop;

  if (result.has_value()) {
    auto &parent = ctx->stack_top();
    auto &caller = *std::prev(parent.current);

    parent.insert(&caller, *result);
  }

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitPHINode(llvm::PHINode &node) {
  auto &frame = ctx->stack_top();

  // PHI nodes in the entry block is invalid.
  DECAF_ASSERT(frame.prev_block != nullptr);

  auto value = frame.lookup(node.getIncomingValueForBlock(frame.prev_block), *z3);
  frame.insert(&node, value);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitCallInst(llvm::CallInst &call) {
  auto func = call.getCalledFunction();

  if (func->isIntrinsic()) {
    DECAF_ABORT(fmt::format("Intrinsic function '{}' not supported", func->getName().str()));
  }

  DECAF_ASSERT(!call.isIndirectCall(), "Indirect functions are not implemented yet");

  if (func->empty())
    return visitExternFunc(call);

  StackFrame callee{func};
  auto &frame = ctx->stack_top();

  for (auto arg_pair :
       boost::combine(boost::make_iterator_range(func->arg_begin(), func->arg_end()),
                      boost::make_iterator_range(call.arg_begin(), call.arg_end()))) {
    callee.insert(&boost::get<0>(arg_pair), frame.lookup(boost::get<1>(arg_pair).get(), *z3));
  }

  ctx->stack.push_back(std::move(callee));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitSelectInst(llvm::SelectInst &inst) {
  auto &frame = ctx->stack_top();

  auto cond = normalize_to_bool(frame.lookup(inst.getCondition(), *z3));
  auto t_val = normalize_to_int(frame.lookup(inst.getTrueValue(), *z3));
  auto f_val = normalize_to_int(frame.lookup(inst.getFalseValue(), *z3));

  frame.insert(&inst, z3::ite(cond, t_val, f_val));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitSExtInst(llvm::SExtInst &sext) {
  auto &frame = ctx->stack_top();

  auto src = normalize_to_int(frame.lookup(sext.getOperand(0), *z3));
  auto dst_ty = sext.getDestTy();

  DECAF_ASSERT(dst_ty->isIntegerTy());
  DECAF_ASSERT(dst_ty->getIntegerBitWidth() > src.get_sort().bv_size());

  auto size_difference = dst_ty->getIntegerBitWidth() - src.get_sort().bv_size();
  frame.insert(&sext, z3::sext(src, size_difference));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitZExtInst(llvm::ZExtInst &zext) {
  auto &frame = ctx->stack_top();

  auto src = normalize_to_int(frame.lookup(zext.getOperand(0), *z3));
  auto dst_ty = zext.getDestTy();

  DECAF_ASSERT(dst_ty->isIntegerTy());
  DECAF_ASSERT(dst_ty->getIntegerBitWidth() > src.get_sort().bv_size());

  auto size_difference = dst_ty->getIntegerBitWidth() - src.get_sort().bv_size();
  frame.insert(&zext, z3::zext(src, size_difference));

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitExternFunc(llvm::CallInst &call) {
  auto func = call.getCalledFunction();
  auto name = func->getName();

  DECAF_ASSERT(func->empty(), "visitExternFunc called with non-external function");

  if (name == "decaf_assert")
    return visitAssert(call);
  if (name == "decaf_assume")
    return visitAssume(call);

  DECAF_ABORT(fmt::format("external function '{}' not implemented", name.str()));
}

ExecutionResult Interpreter::visitAssume(llvm::CallInst &call) {
  DECAF_ASSERT(call.getNumArgOperands() == 1);

  auto &frame = ctx->stack_top();
  ctx->add(normalize_to_bool(frame.lookup(call.getArgOperand(0), *z3)));

  // Don't check whether adding the assumption causes this path to become
  // dead since assumptions are rare, solver calls are expensive, and it'll
  // get caught at the next conditional branch anyway.
  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitAssert(llvm::CallInst &call) {
  DECAF_ASSERT(call.getNumArgOperands() == 1);

  auto &frame = ctx->stack_top();
  auto assertion = normalize_to_bool(frame.lookup(call.getArgOperand(0), *z3));

  DECAF_ASSERT(assertion.is_bool(),
               fmt::format("Called decaf_assert with invalid type, found: {}, expected bool",
                           assertion.get_sort().to_string()));

  if (ctx->check(!assertion) == z3::sat) {
    tracker->add_failure(*ctx, ctx->solver.get_model());
  }
  ctx->add(assertion);

  return ExecutionResult::Continue;
}

ExecutionResult Interpreter::visitInstruction(llvm::Instruction &inst) {
  DECAF_ABORT(fmt::format("Instruction '{}' not implemented!", inst.getOpcodeName()));
}

/************************************************
 * PrintingFailureTracker                       *
 ************************************************/
void PrintingFailureTracker::add_failure(Context &, const z3::model &model) {
  std::cout << "Found failed model! Inputs: \n" << model << std::endl;
}

FailureTracker *PrintingFailureTracker::default_instance() {
  static PrintingFailureTracker instance;

  return &instance;
}

/************************************************
 * Free Functions                               *
 ************************************************/
z3::sort sort_for_type(z3::context &ctx, llvm::Type *type) {
  if (type->isIntegerTy()) {
    return ctx.bv_sort(type->getIntegerBitWidth());
  }

  std::string message;
  llvm::raw_string_ostream os{message};
  os << "Unsupported LLVM type: ";
  type->print(os);

  DECAF_ABORT(message);
}

void execute_symbolic(llvm::Function *function, FailureTracker *tracker) {
  z3::config cfg;

  // We want Z3 to generate models
  cfg.set("model", true);
  // Automatically select and configure the solver
  cfg.set("auto_config", true);

  z3::context z3{cfg};
  Executor exec;

  exec.add_context(Context(z3, function));

  while (exec.has_next()) {
    Context ctx = exec.next_context();
    Interpreter interp{&ctx, &exec, &z3, tracker};

    interp.execute();
  }
}

z3::expr evaluate_constant(z3::context &ctx, llvm::Constant *constant) {
  if (auto *intconst = llvm::dyn_cast<llvm::ConstantInt>(constant)) {
    const llvm::APInt &value = intconst->getValue();

    if (value.getBitWidth() <= 64) {
      return ctx.bv_val(value.getLimitedValue(), value.getBitWidth());
    }

    // This isn't particularly efficient. Unfortunately, when it comes
    // to integers larger than uint64_t there's no efficient way to get
    // them into Z3. The options are either
    //  - Convert to base-10 string and use that
    //  - Put every single bit into a separate boolean then load that
    // I've opted to go the string route since it's easier here. Maybe
    // in the future we can get an API for doing this more efficiently
    // added to Z3.
    llvm::SmallString<64> str;
    value.toStringUnsigned(str, 10);

    return ctx.bv_val(str.c_str(), value.getBitWidth());
  }

  // We only implement integers at the moment
  DECAF_UNIMPLEMENTED();
}

z3::expr normalize_to_bool(const z3::expr &expr) {
  auto sort = expr.get_sort();

  if (sort.is_bv() && sort.bv_size() == 1)
    return expr == 1;

  return expr;
}

z3::expr normalize_to_int(const z3::expr &expr) {
  auto sort = expr.get_sort();

  if (sort.is_bool()) {
    auto &ctx = expr.ctx();
    return z3::ite(expr, ctx.bv_val(1, 1), ctx.bv_val(0, 1));
  }

  return expr;
}
} // namespace decaf


// Copyright (C) 2007-2011 Anders Logg and Garth N. Wells
//
// This file is part of DOLFIN.
//
// DOLFIN 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 3 of the License, or
// (at your option) any later version.
//
// DOLFIN 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. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with DOLFIN. If not, see <http://www.gnu.org/licenses/>.
//
// Modified by Kristian Oelgaard, 2008
// Modified by Martin Sandve Alnes, 2008
// Modified by Johan Hake, 2009
//
// First added:  2007-04-10
// Last changed: 2011-09-19

#include <map>
#include <utility>
#include <boost/assign/list_of.hpp>

#include <dolfin/common/constants.h>
#include <dolfin/common/Array.h>
#include <dolfin/common/NoDeleter.h>
#include <dolfin/function/GenericFunction.h>
#include <dolfin/function/FunctionSpace.h>
#include <dolfin/function/Constant.h>
#include <dolfin/log/log.h>
#include <dolfin/mesh/Mesh.h>
#include <dolfin/mesh/MeshData.h>
#include <dolfin/mesh/MeshDomains.h>
#include <dolfin/mesh/MeshFunction.h>
#include <dolfin/mesh/MeshValueCollection.h>
#include <dolfin/mesh/Vertex.h>
#include <dolfin/mesh/Cell.h>
#include <dolfin/mesh/Facet.h>
#include <dolfin/mesh/Point.h>
#include <dolfin/mesh/SubDomain.h>
#include <dolfin/la/GenericMatrix.h>
#include <dolfin/la/GenericVector.h>
#include <dolfin/la/LinearAlgebraFactory.h>
#include "GenericDofMap.h"
#include "FiniteElement.h"
#include "UFCMesh.h"
#include "UFCCell.h"
#include "DirichletBC.h"

using namespace dolfin;

const std::set<std::string> DirichletBC::methods
            = boost::assign::list_of("topological")("geometric")("pointwise");

//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(const FunctionSpace& V, const GenericFunction& g,
                         const SubDomain& sub_domain, std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(reference_to_no_delete_pointer(g)),
    _method(method), _user_sub_domain(reference_to_no_delete_pointer(sub_domain))
{
  check();
  parameters = default_parameters();
  init_from_sub_domain(_user_sub_domain);
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(boost::shared_ptr<const FunctionSpace> V,
                         boost::shared_ptr<const GenericFunction> g,
                         boost::shared_ptr<const SubDomain> sub_domain,
                         std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(g), _method(method), _user_sub_domain(sub_domain)
{
  check();
  parameters = default_parameters();
  init_from_sub_domain(_user_sub_domain);
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(const FunctionSpace& V, const GenericFunction& g,
                         const MeshFunction<uint>& sub_domains,
                         uint sub_domain, std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(reference_to_no_delete_pointer(g)),
    _method(method)
{
  check();
  parameters = default_parameters();
  init_from_mesh_function(sub_domains, sub_domain);
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(boost::shared_ptr<const FunctionSpace> V,
                         boost::shared_ptr<const GenericFunction> g,
                         boost::shared_ptr<const MeshFunction<uint> > sub_domains,
                         uint sub_domain,
                         std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(g), _method(method)
{
  check();
  parameters = default_parameters();
  init_from_mesh_function(*sub_domains, sub_domain);
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(const FunctionSpace& V, const GenericFunction& g,
                         uint sub_domain, std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(reference_to_no_delete_pointer(g)), _method(method)
{
  check();
  parameters = default_parameters();
  init_from_mesh(sub_domain);
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(boost::shared_ptr<const FunctionSpace> V,
                         boost::shared_ptr<const GenericFunction> g,
                         uint sub_domain, std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(g), _method(method)
{
  check();
  parameters = default_parameters();
  init_from_mesh(sub_domain);
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(boost::shared_ptr<const FunctionSpace> V,
                         boost::shared_ptr<const GenericFunction> g,
                         const std::vector<std::pair<uint, uint> >& markers,
                         std::string method)
  : BoundaryCondition(V),
    Hierarchical<DirichletBC>(*this),
    g(g), _method(method), facets(markers)
{
  check();
  parameters = default_parameters();
}
//-----------------------------------------------------------------------------
DirichletBC::DirichletBC(const DirichletBC& bc)
  : BoundaryCondition(bc._function_space),
    Hierarchical<DirichletBC>(*this)
{
  // Set default parameters
  parameters = default_parameters();

  *this = bc;
}
//-----------------------------------------------------------------------------
DirichletBC::~DirichletBC()
{
  // Do nothing
}
//-----------------------------------------------------------------------------
const DirichletBC& DirichletBC::operator= (const DirichletBC& bc)
{
  _function_space = bc._function_space;
  g = bc.g;
  _method = bc._method;
  _user_sub_domain = bc._user_sub_domain;
  facets = bc.facets;

  // Call assignment operator for base class
  Hierarchical<DirichletBC>::operator=(bc);

  return *this;
}
//-----------------------------------------------------------------------------
void DirichletBC::apply(GenericMatrix& A) const
{
  apply(&A, 0, 0);
}
//-----------------------------------------------------------------------------
void DirichletBC::apply(GenericVector& b) const
{
  apply(0, &b, 0);
}
//-----------------------------------------------------------------------------
void DirichletBC::apply(GenericMatrix& A, GenericVector& b) const
{
  apply(&A, &b, 0);
}
//-----------------------------------------------------------------------------
void DirichletBC::apply(GenericVector& b, const GenericVector& x) const
{
  apply(0, &b, &x);
}
//-----------------------------------------------------------------------------
void DirichletBC::apply(GenericMatrix& A,
                        GenericVector& b,
                        const GenericVector& x) const
{
  apply(&A, &b, &x);
}
//-----------------------------------------------------------------------------
void DirichletBC::get_boundary_values(Map& boundary_values,
                         std::string method) const
{
  // Create local data
  BoundaryCondition::LocalData data(*_function_space);

  // Compute dofs and values
  compute_bc(boundary_values, data, method);
}
//-----------------------------------------------------------------------------
void DirichletBC::zero(GenericMatrix& A) const
{
  // A map to hold the mapping from boundary dofs to boundary values
  Map boundary_values;

  // Create local data for application of boundary conditions
  BoundaryCondition::LocalData data(*_function_space);

  // Compute dofs and values
  compute_bc(boundary_values, data, _method);

  // Copy boundary value data to arrays
  std::vector<uint> dofs(boundary_values.size());
  Map::const_iterator bv;
  uint i = 0;
  for (bv = boundary_values.begin(); bv != boundary_values.end(); ++bv)
    dofs[i++] = bv->first;

  // Modify linear system (A_ii = 1)
  A.zero(boundary_values.size(), &dofs[0]);

  // Finalise changes to A
  A.apply("insert");
}
//-----------------------------------------------------------------------------
void DirichletBC::zero_columns(GenericMatrix& A,
                               GenericVector& b,
                               double diag_val) const
{
  Map bv_map;
  get_boundary_values(bv_map, _method);

  // Create lookup table of dofs
  //const uint nrows = A.size(0); // should be equal to b.size()
  const uint ncols = A.size(1); // should be equal to max possible dof+1

  std::pair<uint,uint> rows = A.local_range(0);
  //std::pair<uint,uint> cols = A.local_range(1);

  std::vector<char> is_bc_dof(ncols);
  std::vector<double> bc_dof_val(ncols);
  for (Map::const_iterator bv = bv_map.begin();  bv != bv_map.end();  ++bv)
  {
    is_bc_dof[bv->first] = 1;
    bc_dof_val[bv->first] = bv->second;
  }

  // Scan through all columns of all rows, setting to zero if is_bc_dof[column]
  // At the same time, we collect corrections to the RHS

  std::vector<uint>   cols;
  std::vector<double> vals;
  std::vector<double> b_vals;
  std::vector<uint>   b_rows;

  for (uint row=rows.first; row<rows.second; row++)
  {
    // If diag_val is nonzero, the matrix is a diagonal block (nrows==ncols),
    // and we can set the whole BC row
    if (diag_val != 0.0 && is_bc_dof[row])
    {
      A.getrow(row, cols, vals);
      for (uint j=0; j<cols.size(); j++)
        vals[j] = (cols[j]==row) * diag_val;
      A.setrow(row, cols, vals);
      A.apply("insert");
      b.setitem(row, bc_dof_val[row]*diag_val);
    }
    else // Otherwise, we scan the row for BC columns
    {
      A.getrow(row, cols, vals);
      bool row_changed=false;
      for (uint j=0; j<cols.size(); j++)
      {
        const uint col = cols[j];

        // Skip columns that aren't BC, and entries that are zero
        if (!is_bc_dof[col] || vals[j] == 0.0)
          continue;

        // We're going to change the row, so make room for it
        if (!row_changed)
        {
          row_changed = true;
          b_rows.push_back(row);
          b_vals.push_back(0.0);
        }

        b_vals.back() -= bc_dof_val[col]*vals[j];
        vals[j] = 0.0;
      }
      if (row_changed)
      {
        A.setrow(row, cols, vals);
        A.apply("insert");
      }
    }
  }

  b.add(&b_vals.front(), b_rows.size(), &b_rows.front());
  b.apply("add");
}
//-----------------------------------------------------------------------------
const std::vector<std::pair<dolfin::uint, dolfin::uint> >& DirichletBC::markers() const
{
  return facets;
}
//-----------------------------------------------------------------------------
boost::shared_ptr<const GenericFunction> DirichletBC::value() const
{
  return g;
}
//-----------------------------------------------------------------------------
boost::shared_ptr<const SubDomain> DirichletBC::user_sub_domain() const
{
  return _user_sub_domain;
}
//-----------------------------------------------------------------------------
bool DirichletBC::is_compatible(GenericFunction& v) const
{
  // This function only checks the values at vertices when it should
  // really check that the dof functionals agree. The check here is
  // neither necessary nor sufficient to guarantee compatible boundary
  // boundary conditions but a more robust test requires access to the
  // function space.

  dolfin_error("DirichletBC.cpp",
               "call is_compatible",
               "This function has not been updated for the new Function class interface");

  /*
  // Compute value size
  uint size = 1;
  const uint rank = g->function_space().element()->value_rank();
  for (uint i = 0; i < rank ; i++)
    size *= g->function_space().element()->value_dimension(i);
  double* g_values = new double[size];
  double* v_values = new double[size];

  // Get mesh
  const Mesh& mesh = _function_space->mesh();

  // Iterate over facets
  for (uint f = 0; f < facets.size(); f++)
  {
    // Create cell and facet
    uint cell_number  = facets[f].first;
    uint facet_number = facets[f].second;
    Cell cell(mesh, cell_number);
    Facet facet(mesh, facet_number);

    // Make cell and facet available to user-defined function
    dolfin_error("DirichletBC.cpp",
                 "add proper message here",
                 "Does the new GenericFunction class need an 'update' function?");
    //g->update(cell, facet_number);
    //v.update(cell, facet_number);

    // Iterate over facet vertices
    for (VertexIterator vertex(facet); !vertex.end(); ++vertex)
    {
      // Evaluate g and v at vertex
      g->eval(g_values, vertex->x());
      v.eval(v_values, vertex->x());

      // Check values
      for (uint i = 0; i < size; i++)
      {
        if (std::abs(g_values[i] - v_values[i]) > DOLFIN_EPS)
        {
          Point p(mesh.geometry().dim(), vertex->x());
          cout << "Incompatible function value " << v_values[i] << " at x = " << p << ", should be " << g_values[i] << "." << endl;
          delete [] g_values;
          delete [] v_values;
          return false;
        }
      }
    }
  }

  delete [] g_values;
  delete [] v_values;
  */

  return true;
}
//-----------------------------------------------------------------------------
void DirichletBC::set_value(const GenericFunction& g)
{
  this->g = reference_to_no_delete_pointer(g);
}
//-----------------------------------------------------------------------------
void DirichletBC::homogenize()
{
  const uint value_rank = g->value_rank();
  if (!value_rank)
  {
    boost::shared_ptr<Constant> zero(new Constant(0.0));
    set_value(zero);
  } else if (value_rank == 1)
  {
    const uint value_dim = g->value_dimension(0);
    std::vector<double> values(value_dim, 0.0);
    boost::shared_ptr<Constant> zero(new Constant(values));
    set_value(zero);
  } else
  {
    std::vector<uint> value_shape;
    for (uint i = 0; i < value_rank; i++)
      value_shape.push_back(g->value_dimension(i));
    std::vector<double> values(g->value_size(), 0.0);
    boost::shared_ptr<Constant> zero(new Constant(value_shape, values));
    set_value(zero);
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::set_value(boost::shared_ptr<const GenericFunction> g)
{
  this->g = g;
}
//-----------------------------------------------------------------------------
std::string DirichletBC::method() const
{
  return _method;
}
//-----------------------------------------------------------------------------
void DirichletBC::apply(GenericMatrix* A,
                        GenericVector* b,
                        const GenericVector* x) const
{
  // Check arguments
  check_arguments(A, b, x);

  // A map to hold the mapping from boundary dofs to boundary values
  Map boundary_values;

  // Create local data for application of boundary conditions
  BoundaryCondition::LocalData data(*_function_space);

  // Compute dofs and values
  compute_bc(boundary_values, data, _method);

  // Copy boundary value data to arrays
  const uint size = boundary_values.size();
  std::vector<uint> dofs(size);
  std::vector<double> values(size);
  Map::const_iterator bv;
  uint i = 0;
  for (bv = boundary_values.begin(); bv != boundary_values.end(); ++bv)
  {
    dofs[i]     = bv->first;
    values[i++] = bv->second;
  }

  // Modify boundary values for nonlinear problems
  if (x)
  {
    // Get values (these must reside in local portion (including ghost
    // values) of the vector
    std::vector<double> x_values(size);
    x->get_local(&x_values[0], dofs.size(), &dofs[0]);

    // Modify RHS entries
    for (uint i = 0; i < size; i++)
      values[i] = x_values[i] - values[i];
  }

  log(PROGRESS, "Applying boundary conditions to linear system.");

  // Modify RHS vector (b[i] = value) and apply changes
  if (b)
  {
    b->set(&values[0], size, &dofs[0]);
    b->apply("insert");
  }

  // Modify linear system (A_ii = 1) and apply changes
  if (A)
  {
    const bool use_ident = parameters["use_ident"];
    if (use_ident)
      A->ident(size, &dofs[0]);
    else
    {
      for (uint i = 0; i < size; i++)
      {
        std::pair<uint, uint> ij(dofs[i], dofs[i]);
        A->setitem(ij, 1.0);
      }
    }

    // Apply changes
    A->apply("add");
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::check() const
{
  dolfin_assert(g);
  dolfin_assert(_function_space->element());

  // Check for common errors, message below might be cryptic
  if (g->value_rank() == 0 && _function_space->element()->value_rank() == 1)
  {
    dolfin_error("DirichletBC.cpp",
                 "create Dirichlet boundary condition",
                 "Expecting a vector-valued boundary value but given function is scalar");
  }
  if (g->value_rank() == 1 && _function_space->element()->value_rank() == 0)
  {
    dolfin_error("DirichletBC.cpp",
                 "create Dirichlet boundary condition",
                 "Expecting a scalar boundary value but given function is vector-valued");
  }

  // Check that value shape of boundary value
  if (g->value_rank() != _function_space->element()->value_rank())
  {
    dolfin_error("DirichletBC.cpp",
                 "create Dirichlet boundary condition",
                 "Illegal value rank (%d), expecting (%d)",
                 g->value_rank(), _function_space->element()->value_rank());
  }
  for (uint i = 0; i < g->value_rank(); i++)
  {
    if (g->value_dimension(i) != _function_space->element()->value_dimension(i))
    {
      dolfin_error("DirichletBC.cpp",
                   "create Dirichlet boundary condition",
                   "Illegal value dimension (%d), expecting (%d)",
                   g->value_dimension(i),
                   _function_space->element()->value_dimension(i));
    }
  }

  // Check that boundary condition method is known
  if (methods.count(_method) == 0)
  {
    dolfin_error("DirichletBC.cpp",
                 "create Dirichlet boundary condition",
                 "unknown method (\"%s\")", _method.c_str());
  }

  // Check that the mesh is ordered
  dolfin_assert(_function_space->mesh());
  if (!_function_space->mesh()->ordered())
  {
    dolfin_error("DirichletBC.cpp",
                 "create Dirichlet boundary condition",
                 "Mesh is not ordered according to the UFC numbering convention. Consider calling mesh.order()");
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::init_from_sub_domain(boost::shared_ptr<const SubDomain> sub_domain)
{
  dolfin_assert(facets.size() == 0);

  // FIXME: This can be made more efficient, we should be able to
  // FIXME: extract the facets without first creating a MeshFunction on
  // FIXME: the entire mesh and then extracting the subset. This is done
  // FIXME: mainly for convenience (we may reuse mark() in SubDomain).

  dolfin_assert(_function_space->mesh());
  const Mesh& mesh = *_function_space->mesh();

  // Create mesh function for sub domain markers on facets
  const uint dim = mesh.topology().dim();
  _function_space->mesh()->init(dim - 1);
  MeshFunction<uint> sub_domains(mesh, dim - 1);

  // Set geometric dimension (needed for SWIG interface)
  sub_domain->_geometric_dimension = mesh.geometry().dim();

  // Mark everything as sub domain 1
  sub_domains = 1;

  // Mark the sub domain as sub domain 0
  sub_domain->mark(sub_domains, 0);

  // Initialize from mesh function
  init_from_mesh_function(sub_domains, 0);
}
//-----------------------------------------------------------------------------
void DirichletBC::init_from_mesh_function(const MeshFunction<uint>& sub_domains,
                                          uint sub_domain)
{
  dolfin_assert(facets.size() == 0);

  dolfin_assert(_function_space->mesh());
  const Mesh& mesh = *_function_space->mesh();

  // Make sure we have the facet - cell connectivity
  const uint dim = mesh.topology().dim();
  mesh.init(dim - 1, dim);

  // Build set of boundary facets
  for (FacetIterator facet(mesh); !facet.end(); ++facet)
  {
    // Skip facets not on this boundary
    if (sub_domains[*facet] != sub_domain)
      continue;

    // Get cell to which facet belongs (there may be two, but pick first)
    const Cell cell(mesh, facet->entities(dim)[0]);

    // Get local index of facet with respect to the cell
    const uint facet_number = cell.index(*facet);

    // Copy data
    facets.push_back(std::pair<uint, uint>(cell.index(), facet_number));
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::init_from_mesh(uint sub_domain)
{
  dolfin_assert(facets.size() == 0);

  // For this to work, the mesh *needs* to be ordered according to
  // the UFC ordering before it gets here. So reordering the mesh
  // here will either have no effect (if the mesh is already ordered
  // or it won't do anything good (since the markers are wrong anyway).
  // In conclusion: we don't need to order the mesh here.

  dolfin_assert(_function_space->mesh());
  const Mesh& mesh = *_function_space->mesh();

  // Assign domain numbers for each facet
  const uint D = mesh.topology().dim();
  const std::map<std::pair<uint, uint>, uint>&
    markers = mesh.domains().markers(D - 1).values();
  std::map<std::pair<uint, uint>, uint>::const_iterator mark;
  for (mark = markers.begin(); mark != markers.end(); ++mark)
  {
    if (mark->second == sub_domain)
      facets.push_back(mark->first);
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::compute_bc(Map& boundary_values,
                             BoundaryCondition::LocalData& data,
                             std::string method) const
{
  // Set method if dafault
  if (method == "default")
    method = _method;

  // Choose strategy
  if (method == "topological")
    compute_bc_topological(boundary_values, data);
  else if (method == "geometric")
    compute_bc_geometric(boundary_values, data);
  else if (method == "pointwise")
    compute_bc_pointwise(boundary_values, data);
  else
  {
    dolfin_error("DirichletBC.cpp",
                 "compute boundary conditions",
                 "Unknown method for application of boundary conditions");
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::compute_bc_topological(Map& boundary_values,
                                         BoundaryCondition::LocalData& data) const
{
  dolfin_assert(_function_space);
  dolfin_assert(g);

  // Special case
  if (facets.size() == 0)
  {
    if (MPI::num_processes() == 1)
      warning("Found no facets matching domain for boundary condition.");
    return;
  }

  // Get mesh and dofmap
  dolfin_assert(_function_space->mesh());
  dolfin_assert(_function_space->dofmap());
  const Mesh& mesh = *_function_space->mesh();
  const GenericDofMap& dofmap = *_function_space->dofmap();

  // Create UFC cell object
  UFCCell ufc_cell(mesh);

  // Iterate over facets
  dolfin_assert(_function_space->element());
  Progress p("Computing Dirichlet boundary values, topological search", facets.size());
  for (uint f = 0; f < facets.size(); ++f)
  {
    // Get cell number and local facet number
    const uint cell_number  = facets[f].first;
    const uint facet_number = facets[f].second;

    // Create cell
    Cell cell(mesh, cell_number);
    ufc_cell.update(cell, facet_number);

    // Restrict coefficient to cell
    g->restrict(&data.w[0], *_function_space->element(), cell, ufc_cell);

    // Tabulate dofs on cell
    dofmap.tabulate_dofs(&data.cell_dofs[0], cell);

    // Tabulate which dofs are on the facet
    dofmap.tabulate_facet_dofs(&data.facet_dofs[0], facet_number);

    // Pick values for facet
    for (uint i = 0; i < dofmap.num_facet_dofs(); i++)
    {
      const uint global_dof = data.cell_dofs[data.facet_dofs[i]];
      const double value = data.w[data.facet_dofs[i]];
      boundary_values[global_dof] = value;
    }

    p++;
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::compute_bc_geometric(Map& boundary_values,
                                      BoundaryCondition::LocalData& data) const
{
  dolfin_assert(_function_space);
  dolfin_assert(_function_space->element());
  dolfin_assert(g);

  // Special case
  if (facets.size() == 0)
  {
    if (MPI::num_processes() == 1)
      warning("Found no facets matching domain for boundary condition.");
    return;
  }

  // Get mesh and dofmap
  dolfin_assert(_function_space->mesh());
  dolfin_assert(_function_space->dofmap());
  const Mesh& mesh = *_function_space->mesh();
  const GenericDofMap& dofmap = *_function_space->dofmap();

  // Initialize facets, needed for geometric search
  log(TRACE, "Computing facets, needed for geometric application of boundary conditions.");
  mesh.init(mesh.topology().dim() - 1);

  // Iterate over facets
  Progress p("Computing Dirichlet boundary values, geometric search", facets.size());
  for (uint f = 0; f < facets.size(); ++f)
  {
    // Get cell number and local facet number
    const uint cell_number = facets[f].first;
    const uint facet_number = facets[f].second;

    // Create facet
    Cell cell(mesh, cell_number);
    Facet facet(mesh, cell.entities(mesh.topology().dim() - 1)[facet_number]);

    // Create UFC cell object
    UFCCell ufc_cell(mesh);

    // Loop the vertices associated with the facet
    for (VertexIterator vertex(facet); !vertex.end(); ++vertex)
    {
      // Loop the cells associated with the vertex
      for (CellIterator c(*vertex); !c.end(); ++c)
      {
        ufc_cell.update(*c, facet_number);

        bool interpolated = false;

        // Tabulate coordinates of dofs on cell
        dofmap.tabulate_coordinates(data.coordinates, ufc_cell);

        // Loop over all dofs on cell
        for (uint i = 0; i < dofmap.cell_dimension(c->index()); ++i)
        {
          // Check if the coordinates are on current facet and thus on boundary
          if (!on_facet(&(data.coordinates[i][0]), facet))
            continue;

          if (!interpolated)
          {
            // Tabulate dofs on cell
            dofmap.tabulate_dofs(&data.cell_dofs[0], *c);

            // Restrict coefficient to cell
            g->restrict(&data.w[0], *_function_space->element(), cell, ufc_cell);
          }

          // Set boundary value
          const uint global_dof = data.cell_dofs[i];
          const double value = data.w[i];
          boundary_values[global_dof] = value;
        }
      }
    }
  }
}
//-----------------------------------------------------------------------------
void DirichletBC::compute_bc_pointwise(Map& boundary_values,
                                      BoundaryCondition::LocalData& data) const
{
  dolfin_assert(_function_space);
  dolfin_assert(_function_space->element());
  dolfin_assert(g);
  dolfin_assert(_user_sub_domain);

  // Get mesh and dofmap
  dolfin_assert(_function_space->mesh());
  dolfin_assert(_function_space->dofmap());
  const Mesh& mesh = *_function_space->mesh();
  const GenericDofMap& dofmap = *_function_space->dofmap();

  // Geometric dim
  const uint gdim = mesh.geometry().dim();

  // Create UFC cell object
  UFCCell ufc_cell(mesh);

  // Iterate over cells
  Progress p("Computing Dirichlet boundary values, pointwise search", mesh.num_cells());
  Array<double> x(gdim);
  for (CellIterator cell(mesh); !cell.end(); ++cell)
  {
    // Update UFC cell
    ufc_cell.update(*cell);

    // Tabulate coordinates of dofs on cell
    dofmap.tabulate_coordinates(data.coordinates, ufc_cell);

    // Interpolate function only once and only on cells where necessary
    bool already_interpolated = false;

    // Loop all dofs on cell
    for (uint i = 0; i < dofmap.cell_dimension(cell->index()); ++i)
    {
      // Check if the coordinates are part of the sub domain (calls user-defined 'indside' function)
      for (uint j = 0; j < gdim; ++j)
        x[j] = data.coordinates[i][j];
      if (!_user_sub_domain->inside(x, false))
        continue;

      if (!already_interpolated)
      {
        already_interpolated = true;

        // Tabulate dofs on cell
        dofmap.tabulate_dofs(&data.cell_dofs[0], *cell);

        // Restrict coefficient to cell
        g->restrict(&data.w[0], *_function_space->element(), *cell, ufc_cell);
      }

      // Set boundary value
      const uint global_dof = data.cell_dofs[i];
      const double value = data.w[i];
      boundary_values[global_dof] = value;
    }

    p++;
  }
}
//-----------------------------------------------------------------------------
bool DirichletBC::on_facet(double* coordinates, Facet& facet) const
{
  // Check if the coordinates are on the same line as the line segment
  if (facet.dim() == 1)
  {
    // Create points
    Point p(coordinates[0], coordinates[1]);
    const Point v0 = Vertex(facet.mesh(), facet.entities(0)[0]).point();
    const Point v1 = Vertex(facet.mesh(), facet.entities(0)[1]).point();

    // Create vectors
    const Point v01 = v1 - v0;
    const Point vp0 = v0 - p;
    const Point vp1 = v1 - p;

    // Check if the length of the sum of the two line segments vp0 and vp1 is
    // equal to the total length of the facet
    if ( std::abs(v01.norm() - vp0.norm() - vp1.norm()) < DOLFIN_EPS )
      return true;
    else
      return false;
  }
  // Check if the coordinates are in the same plane as the triangular facet
  else if (facet.dim() == 2)
  {
    // Create points
    const Point p(coordinates[0], coordinates[1], coordinates[2]);
    const Point v0 = Vertex(facet.mesh(), facet.entities(0)[0]).point();
    const Point v1 = Vertex(facet.mesh(), facet.entities(0)[1]).point();
    const Point v2 = Vertex(facet.mesh(), facet.entities(0)[2]).point();

    // Create vectors
    const Point v01 = v1 - v0;
    const Point v02 = v2 - v0;
    const Point vp0 = v0 - p;
    const Point vp1 = v1 - p;
    const Point vp2 = v2 - p;

    // Check if the sum of the area of the sub triangles is equal to the total
    // area of the facet
    if (std::abs(v01.cross(v02).norm() - vp0.cross(vp1).norm() - vp1.cross(vp2).norm()
        - vp2.cross(vp0).norm()) < DOLFIN_EPS)
      return true;
    else
      return false;
  }

  dolfin_error("DirichletBC.cpp",
               "determine if given point is on facet",
               "Not implemented for given facet dimension");

  return false;
}
//-----------------------------------------------------------------------------


/*
 * Copyright (c) 2014, Stanislav Vorobiov
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "HermitePath.h"
#include "Settings.h"
#include <boost/make_shared.hpp>
#include <cmath>

namespace af
{
    HermitePathIterator::HermitePathIterator(const HermitePath* path, int i, float pos)
    : path_(path),
      i_(i),
      pos_(pos)
    {
        if (path_->lengths_.empty()) {
            return;
        }

        if (i < 0) {
            i = 0;
            pos = 0.0f;
        }

        if (static_cast<size_t>(i) >= path_->lengths_.size()) {
            i = path_->lengths_.size() - 1;
            pos = path_->lengths_[i];
        }

        float t = pos / path_->lengths_[i];
        point_ = path_->samples_[i] + t * (path_->samples_[i + 1] - path_->samples_[i]);
    }

    HermitePathIterator::~HermitePathIterator()
    {
    }

    PathIteratorPtr HermitePathIterator::clone() const
    {
        return boost::make_shared<HermitePathIterator>(path_, i_, pos_);
    }

    const b2Vec2& HermitePathIterator::current() const
    {
        return point_;
    }

    bool HermitePathIterator::less(const PathIteratorPtr& other) const
    {
        const HermitePathIteratorPtr& r =
            boost::static_pointer_cast<HermitePathIterator>(other);

        return less(*r);
    }

    bool HermitePathIterator::eq(const PathIteratorPtr& other) const
    {
        const HermitePathIteratorPtr& r =
            boost::static_pointer_cast<HermitePathIterator>(other);

        return eq(*r);
    }

    void HermitePathIterator::step(float length)
    {
        if ((length == 0.0f) || path_->lengths_.empty()) {
            return;
        }

        if (i_ < 0) {
            if (length > 0.0f) {
                i_ = 0;
                pos_ = 0.0f;
            } else {
                return;
            }
        }

        if (static_cast<size_t>(i_) >= path_->lengths_.size()) {
            if (length > 0.0f) {
                return;
            } else {
                i_ = path_->lengths_.size() - 1;
                pos_ = path_->lengths_[i_];
            }
        }

        if (length > 0.0f) {
            stepForward(length);
        } else {
            stepBack(-length);
        }
    }

    void HermitePathIterator::stepForward(float length)
    {
        if (length > path_->length_) {
            if (loop()) {
                length = std::fmod(length, path_->length_);
                if (length == 0.0f) {
                    return;
                }
            } else {
                i_ = path_->lengths_.size();
                pos_ = 0.0f;
                point_ = path_->samples_[i_];
                return;
            }
        }

        if (loop()) {
            float l = 0.0f;
            float tmpPos = pos_ + length;
            size_t k = i_;

            while (true) {
                if (tmpPos <= (l + path_->lengths_[k])) {
                    i_ = k;
                    pos_ = tmpPos - l;

                    float t = pos_ / path_->lengths_[i_];
                    point_ = path_->samples_[i_] + t * (path_->samples_[i_ + 1] - path_->samples_[i_]);

                    return;
                }
                l += path_->lengths_[k];

                ++k;
                k %= path_->lengths_.size();
            }
        } else {
            float l = 0.0f;
            float tmpPos = pos_ + length;

            for (size_t k = i_; k < path_->lengths_.size(); ++k) {
                if (tmpPos <= (l + path_->lengths_[k])) {
                    i_ = k;
                    pos_ = tmpPos - l;

                    float t = pos_ / path_->lengths_[i_];
                    point_ = path_->samples_[i_] + t * (path_->samples_[i_ + 1] - path_->samples_[i_]);

                    return;
                }
                l += path_->lengths_[k];
            }

            i_ = path_->lengths_.size();
            pos_ = 0.0f;
            point_ = path_->samples_[i_];
        }
    }

    void HermitePathIterator::stepBack(float length)
    {
        if (length > path_->length_) {
            if (loop()) {
                length = std::fmod(length, path_->length_);
                if (length == 0.0f) {
                    return;
                }
            } else {
                i_ = -1;
                pos_ = 0.0f;
                point_ = path_->samples_[0];
                return;
            }
        }

        if (loop()) {
            float l = 0.0f;
            float tmpPos = path_->lengths_[i_] + length - pos_;
            size_t k = i_;

            while (true) {
                l += path_->lengths_[k];
                if (tmpPos < l) {
                    i_ = k;
                    pos_ = l - tmpPos;

                    float t = pos_ / path_->lengths_[i_];
                    point_ = path_->samples_[i_] + t * (path_->samples_[i_ + 1] - path_->samples_[i_]);

                    return;
                }

                if (k-- == 0) {
                    k = path_->lengths_.size() - 1;
                }
            }
        } else {
            float l = 0.0f;
            float tmpPos = path_->lengths_[i_] + length - pos_;

            for (int k = i_; k >= 0; --k) {
                l += path_->lengths_[k];
                if (tmpPos < l) {
                    i_ = k;
                    pos_ = l - tmpPos;

                    float t = pos_ / path_->lengths_[i_];
                    point_ = path_->samples_[i_] + t * (path_->samples_[i_ + 1] - path_->samples_[i_]);

                    return;
                }
            }

            i_ = -1;
            pos_ = 0.0f;
            point_ = path_->samples_[0];
        }
    }

    HermitePath::HermitePath(int numIterations, float c)
    : step_(1.0f / numIterations),
      c_(c),
      length_(0.0f)
    {
        if (numIterations <= 0) {
            step_ = 1.0f;
        }
    }

    HermitePath::HermitePath(float c)
    : step_(1.0f / settings.hermitePath.numIterations),
      c_(c),
      length_(0.0f)
    {
    }

    HermitePath::~HermitePath()
    {
    }

    void HermitePath::add(const b2Vec2& point)
    {
        p_.push_back(point);

        if (p_.size() >= 3) {
            const b2Vec2& p2 = p_[p_.size() - 1];
            const b2Vec2& p0 = p_[p_.size() - 3];
            m_.push_back((1.0f - c_) * 0.5f * (p2 - p0));
        }

        if (p_.size() >= 4) {
            b2Vec2 prev = at(p_.size() - 4, 0.0f);

            if (p_.size() == 4) {
                samples_.push_back(prev);
            }

            for (float t = step_; t <= (1.0f - step_); t += step_) {
                b2Vec2 cur = at(p_.size() - 4, t);
                samples_.push_back(cur);

                lengths_.push_back(b2Distance(prev, cur));
                length_ += lengths_.back();

                prev = cur;
            }

            b2Vec2 cur = at(p_.size() - 4, 1.0f);
            samples_.push_back(cur);

            lengths_.push_back(b2Distance(prev, cur));
            length_ += lengths_.back();
        }
    }

    void HermitePath::add(const b2Vec2* points, size_t numPoints)
    {
        for (size_t i = 0; i < numPoints; ++i) {
            add(points[i]);
        }
    }

    void HermitePath::addFront(const b2Vec2& point)
    {
        p_.insert(p_.begin(), point);

        if (p_.size() >= 3) {
            const b2Vec2& p2 = p_[2];
            const b2Vec2& p0 = p_[0];
            m_.insert(m_.begin(), (1.0f - c_) * 0.5f * (p2 - p0));
        }

        if (p_.size() >= 4) {
            b2Vec2 prev = at(0, 0.0f);

            std::vector<float> tmpLengths;
            Points tmpSamples;

            if (p_.size() == 4) {
                tmpSamples.push_back(prev);
            }

            for (float t = step_; t <= (1.0f - step_); t += step_) {
                b2Vec2 cur = at(0, t);
                tmpSamples.push_back(cur);

                tmpLengths.push_back(b2Distance(prev, cur));
                length_ += tmpLengths.back();

                prev = cur;
            }

            b2Vec2 cur = at(0, 1.0f);
            tmpSamples.push_back(cur);

            tmpLengths.push_back(b2Distance(prev, cur));
            length_ += tmpLengths.back();

            samples_.insert(samples_.begin(), tmpSamples.begin(), tmpSamples.end());
            lengths_.insert(lengths_.begin(), tmpLengths.begin(), tmpLengths.end());
        }
    }

    void HermitePath::addFront(const b2Vec2* points, size_t numPoints)
    {
        for (size_t i = 0; i < numPoints; ++i) {
            addFront(points[i]);
        }
    }

    PathPtr HermitePath::clone() const
    {
        HermitePathPtr p = boost::make_shared<HermitePath>(1.0f, 0.0f);

        /*
         * FIXME: hack.
         */
        p->step_ = step_;
        p->c_ = c_;
        p->p_ = p_;
        p->m_ = m_;
        p->length_ = length_;
        p->lengths_ = lengths_;
        p->samples_ = samples_;

        return p;
    }

    const Points& HermitePath::points() const
    {
        return p_;
    }

    float HermitePath::length() const
    {
        return length_;
    }

    PathIteratorPtr HermitePath::first() const
    {
        return boost::make_shared<HermitePathIterator>(this, 0, 0.0f);
    }

    PathIteratorPtr HermitePath::last() const
    {
        if (lengths_.empty()) {
            return boost::make_shared<HermitePathIterator>(this, 0, 0.0f);
        } else {
            return boost::make_shared<HermitePathIterator>(this, lengths_.size() - 1, lengths_.back());
        }
    }

    PathIteratorPtr HermitePath::end() const
    {
        if (lengths_.empty()) {
            return boost::make_shared<HermitePathIterator>(this, 0, 0.0f);
        } else {
            return boost::make_shared<HermitePathIterator>(this, lengths_.size(), 0.0f);
        }
    }

    PathIteratorPtr HermitePath::rend() const
    {
        if (lengths_.empty()) {
            return boost::make_shared<HermitePathIterator>(this, 0, 0.0f);
        } else {
            return boost::make_shared<HermitePathIterator>(this, -1, 0.0f);
        }
    }

    PathIteratorPtr HermitePath::find(float pos) const
    {
        if (lengths_.empty()) {
            return boost::make_shared<HermitePathIterator>(this, 0, 0.0f);
        }

        if (pos < 0.0f) {
            return boost::make_shared<HermitePathIterator>(this, -1, 0.0f);
        }

        float l = 0.0f;

        for (size_t i = 0; i < lengths_.size(); ++i) {
            if (pos <= (l + lengths_[i])) {
                return boost::make_shared<HermitePathIterator>(this, i, pos - l);
            }
            l += lengths_[i];
        }

        return boost::make_shared<HermitePathIterator>(this, lengths_.size(), 0.0f);
    }

    HermitePathIterator HermitePath::firstIt() const
    {
        return HermitePathIterator(this, 0, 0.0f);
    }

    HermitePathIterator HermitePath::lastIt() const
    {
        if (lengths_.empty()) {
            return HermitePathIterator(this, 0, 0.0f);
        } else {
            return HermitePathIterator(this, lengths_.size() - 1, lengths_.back());
        }
    }

    HermitePathIterator HermitePath::findIt(float pos) const
    {
        if (lengths_.empty()) {
            return HermitePathIterator(this, 0, 0.0f);
        }

        if (pos < 0.0f) {
            return HermitePathIterator(this, -1, 0.0f);
        }

        float l = 0.0f;

        for (size_t i = 0; i < lengths_.size(); ++i) {
            if (pos <= (l + lengths_[i])) {
                return HermitePathIterator(this, i, pos - l);
            }
            l += lengths_[i];
        }

        return HermitePathIterator(this, lengths_.size(), 0.0f);
    }

    HermitePathIterator HermitePath::endIt() const
    {
        if (lengths_.empty()) {
            return HermitePathIterator(this, 0, 0.0f);
        } else {
            return HermitePathIterator(this, lengths_.size(), 0.0f);
        }
    }

    HermitePathIterator HermitePath::rendIt() const
    {
        if (lengths_.empty()) {
            return HermitePathIterator(this, 0, 0.0f);
        } else {
            return HermitePathIterator(this, -1, 0.0f);
        }
    }

    void HermitePath::clear()
    {
        p_.clear();
        m_.clear();
        length_ = 0.0f;
        lengths_.clear();
        samples_.clear();
    }

    b2Vec2 HermitePath::at(int i, float t) const
    {
        float h00 = 2 * t * t * t - 3 * t * t + 1;
        float h10 = t * t * t - 2 * t * t + t;
        float h01 = - 2 * t * t * t + 3 * t * t;
        float h11 = t * t * t - t * t;

        const b2Vec2& p0 = p_[i + 1];
        const b2Vec2& p1 = p_[i + 2];

        const b2Vec2& m0 = m_[i];
        const b2Vec2& m1 = m_[i + 1];

        return h00 * p0 + h10 * m0 + h01 * p1 + h11 * m1;
    }
}


#include <stdgl/ShaderProgram.hpp>
#include <stdgl/VoxelData.hpp>
#include <stdgl/Texture3D.hpp>
#include <csgframework/CsgTree.hpp>
#include <csgframework/CsgLoader.hpp>

#include <stdio.h>
#include <iostream>
#include <memory>

#include <imgui/imgui.h>
#include <imgui/imgui_impl_glfw.h>
#include <GLFW/glfw3.h>

#include <Eigen/Geometry>

#define TO_STRING(x) #x

#define SHADER_PREFIX \
  "#version 120\n" \
  "#define MAXFLOAT 1.0e15f\n"

const char* vs_str = SHADER_PREFIX TO_STRING
(
  //! Normalized pixel coordinates.
  varying vec2 vPixel;

  void main (void)
  {
    vPixel = gl_Vertex.xy;

    gl_Position = gl_Vertex;
  }
);

const char* fs_str = SHADER_PREFIX TO_STRING
(
  //! Normalized pixel coordinates.
  varying vec2 vPixel;

  uniform vec3 iResolution;
  uniform float iGlobalTime;
  uniform float iCameraAngle;
  uniform float iCameraDistance;

  uniform float iSceneRadius;

  //! Scene epsilon to prevent self-intersections.
  uniform float iSceneEpsilon;

  uniform sampler3D iDistanceTexture;

  uniform vec3 iBoundsMin;
  uniform vec3 iBoundsMax;

  float sdPlane (vec3 p)
  {
    return p.y;
  }

  //----------------------------------------------------------------------

  vec2 opU (vec2 d1, vec2 d2)
  {
    return (d1.x < d2.x) ? d1 : d2;
  }

  //----------------------------------------------------------------------

  float distanceField (in vec3 thePoint)
  {
    vec3 aTexCoord = (thePoint - iBoundsMin) / (iBoundsMax - iBoundsMin);

    return texture3D (iDistanceTexture, aTexCoord).r;
  }

  vec2 map (in vec3 thePos)
  {
    vec2 res = vec2 (distanceField (thePos), 46.9);
    res = opU (vec2 (sdPlane (thePos - vec3( 0.0, iBoundsMin.y, 0.0)), 1.0), res);

    return res;
  }

  vec2 castRayTexture (in vec3 ro, in vec3 rd, in vec2 theRange)
  {
    float tmin = theRange.x;
    float tmax = theRange.y;

    float precis = iSceneEpsilon;
    float t = tmin;
    float m = -1.0;

    for (int i = 0; i < 100; i++)
    {
      vec3 aPos = ro + rd * t;
      vec2 res = map (aPos);

      if (res.x < precis || t > tmax)
      {
        break;
      }

      t += res.x;
      m = res.y;
    }

    if (t > tmax)
    {
      m = -1.0;
    }

    return vec2 (t, m);
  }

  float softshadow (in vec3 ro, in vec3 rd, in float mint, in float tmax)
  {
    float res = 1.0;
    float t = mint;

    for (int i = 0; i < 16; i++)
    {
      float h = map (ro + rd * t).x;
      res = min (res, 8.0 * h / t);
      t += clamp (h, 0.02, 0.10);

      if (h < iSceneEpsilon || t > tmax)
      {
        break;
      }
    }

    return clamp (res, 0.0, 1.0);
  }

  vec3 calcNormal (in vec3 pos)
  {
    vec3 eps = vec3 (iSceneEpsilon, 0.0, 0.0);
    vec3 nor = vec3 ( map (pos + eps.xyy).x - map (pos - eps.xyy).x,
                      map (pos + eps.yxy).x - map (pos - eps.yxy).x,
                      map (pos + eps.yyx).x - map (pos - eps.yyx).x);
    return normalize (nor);
  }

  float calcAO (in vec3 pos, in vec3 nor)
  {
    float occ = 0.0;
    float sca = 1.0;

    for (int i = 0; i < 5; i++)
    {
      float hr = 0.01 + 0.12 * float (i) / 4.0;
      vec3 aopos =  nor * hr + pos;
      float dd = map (aopos).x;
      occ += - (dd - hr) * sca;
      sca *= 0.95;
    }

    return clamp (1.0 - 3.0 * occ, 0.0, 1.0);
  }

  vec3 render (in vec3 ro, in vec3 rd, in vec2 theRange)
  {
    vec3 col = vec3 (0.7, 0.9, 1.0) + rd.y * 0.8;

    if (theRange.x == MAXFLOAT)
    {
      return col;
    }

    vec2 res = castRayTexture (ro, rd, theRange);

    float t = res.x;
    float m = res.y;

    if (m > -0.5)
    {
      vec3 pos = ro + t * rd;
      vec3 nor = calcNormal (pos);
      vec3 ref = reflect (rd, nor);

      // col = abs(nor);
      // return col;

      // material
      col = 0.45 + 0.3 * sin (vec3 (0.05, 0.08, 0.10) * (m - 1.0));

      if (m < 1.5)
      {
        float f = mod (floor (5.0 * pos.z) + floor (5.0 * pos.x), 2.0);
        col = 0.4 + 0.1 * f * vec3 (1.0);
      }

      // lighitng
      float occ = calcAO (pos, nor);
      vec3  lig = normalize (vec3 (-0.6, 0.7, -0.5));
      float amb = clamp (0.5 + 0.5 * nor.y, 0.0, 1.0);
      float dif = clamp (dot (nor, lig), 0.0, 1.0);
      float bac = clamp (dot (nor, normalize (vec3 (-lig.x, 0.0, -lig.z))), 0.0, 1.0) * clamp (1.0 - pos.y, 0.0, 1.0);
      float dom = smoothstep (-0.1, 0.1, ref.y);
      float fre = pow (clamp (1.0 + dot (nor, rd), 0.0, 1.0), 2.0);
      float spe = pow (clamp (dot (ref, lig), 0.0, 1.0), 16.0);

      // dif *= softshadow (pos, lig, 0.02, 2.5);
      // dom *= softshadow( pos, ref, 0.02, 2.5 );

      vec3 lin = vec3 (0.0);
      lin += 1.20 * dif * vec3 (1.00, 0.85, 0.55);
      lin += 1.20 * spe * vec3 (1.00, 0.85, 0.55) * dif;
      lin += 0.20 * amb * vec3 (0.50, 0.70, 1.00) * occ;
      lin += 0.30 * dom * vec3 (0.50, 0.70, 1.00) * occ;
      lin += 0.30 * bac * vec3 (0.25, 0.25, 0.25) * occ;
      lin += 0.40 * fre * vec3 (1.00, 1.00, 1.00) * occ;
      col = col * lin;
    }

    return vec3 (clamp (col, 0.0, 1.0));
  }

  mat3 setCamera (in vec3 ro, in vec3 ta, float cr)
  {
    vec3 cw = normalize (ta - ro);
    vec3 cp = vec3 (sin (cr), cos (cr), 0.0);
    vec3 cu = normalize (cross (cw, cp));
    vec3 cv = normalize (cross (cu, cw));
    return mat3 (cu, cv, cw);
  }

  vec2 intersectBox (in vec3 theOrigin, in vec3 theDirect, in vec3 theMinPnt, in vec3 theMaxPnt)
  {
    vec3 aTimeBoxMin = (theMinPnt - theOrigin) * (1.f / theDirect);
    vec3 aTimeBoxMax = (theMaxPnt - theOrigin) * (1.f / theDirect);

    vec3 aTimeMax = max (aTimeBoxMin, aTimeBoxMax);
    vec3 aTimeMin = min (aTimeBoxMin, aTimeBoxMax);

    float aTime1 = max (aTimeMin.x, max (aTimeMin.y, aTimeMin.z));
    float aTime2 = min (aTimeMax.x, min (aTimeMax.y, aTimeMax.z));

    return aTime1 > aTime2 || aTime2 < 0.f ?
           vec2 (MAXFLOAT) : vec2 (max (aTime1, 0.f), aTime2);
  }

  void main()
  {

    vec2 p = vPixel;
    p.x *= iResolution.x / iResolution.y;

    float time = 15.0f + iGlobalTime;

    // camera
    vec3 ta = (iBoundsMin + iBoundsMax) * 0.5f;
    float R = iCameraDistance;
    vec3 ro = ta + vec3 (R * cos (0.1f * time + iCameraAngle),
                         R * 0.5f,
                         R * sin (0.1f * time + iCameraAngle));

    // camera-to-world transformation
    mat3 ca = setCamera (ro, ta, 0.0f);

    // ray direction
    vec3 rd = ca * normalize (vec3 (p.xy, 2.0f));

    vec2 aRange = intersectBox (ro, rd, iBoundsMin, iBoundsMax);

    // render
    vec3 col = render (ro, rd, aRange);

    col = pow (col, vec3 (0.4545f));

    gl_FragColor = vec4 (col, 1.0f);
  }

);

#define MAXFLOAT 1.0e15f

namespace
{
  float evalPrimitiveDistance (const Vec4f& thePos, CsgPrimitiveNode* theNode)
  {
    float aDistance = MAXFLOAT;
    Vec3f aScaling;

    Mat4f aMatWithoutScale = theNode->Transform();
    for (int i = 0; i < 3; ++i)
    {
      float aScaleI = aMatWithoutScale.col (i).head<3>().norm();
      aScaling[i] = aScaleI;
      aMatWithoutScale.col (i).head<3>() *= 1.f / aScaleI;
    }

    Vec4f aTransformedPos = aMatWithoutScale.inverse() * thePos;
    aTransformedPos /= aTransformedPos.w();
    const Vec3f& aPoint = aTransformedPos.head<3>();

    switch (theNode->TypeID())
    {
      case CSG_SPHERE:
      {
        aDistance = aPoint.norm() - aScaling.x(); // only uniform scaling for spheres
        break;
      }
      case CSG_BOX:
      {
        Vec3f d = aPoint.cwiseAbs() - aScaling;
        aDistance = std::min (std::max (d.x(), std::max (d.y(), d.z())), 0.f)
                      + (d.cwiseMax (Vec3f (0.f, 0.f, 0.f))).norm();
        break;
      }
    }

    return aDistance;
  }

  float evalDistance (const Vec4f& thePos, CsgNode* theNode)
  {
    if (theNode->IsLeaf())
    {
      return evalPrimitiveDistance (thePos, static_cast<CsgPrimitiveNode*> (theNode));
    }

    float aDistance = MAXFLOAT;
    CsgOperationNode* anOpNode = static_cast<CsgOperationNode*> (theNode);

    switch (theNode->TypeID())
    {
      case CSG_OP_UNION:
      {
        aDistance = std::min (evalDistance (thePos, anOpNode->Child<0>()),
                              evalDistance (thePos, anOpNode->Child<1>()));
        break;
      }
      case CSG_OP_INTER:
      {
        aDistance = std::max (evalDistance (thePos, anOpNode->Child<0>()),
                              evalDistance (thePos, anOpNode->Child<1>()));
        break;
      }
      case CSG_OP_MINUS:
      {
        aDistance = std::max ( evalDistance (thePos, anOpNode->Child<0>()),
                              -evalDistance (thePos, anOpNode->Child<1>()));
        break;
      }
    }

    return aDistance;
  }

  static const GLfloat aQuadVertices[] = { -1.f, -1.f,  0.f,
                                           -1.f,  1.f,  0.f,
                                            1.f,  1.f,  0.f,
                                            1.f,  1.f,  0.f,
                                            1.f, -1.f,  0.f,
                                           -1.f, -1.f,  0.f
                                         };

  static void errorCallback (int error, const char* description)
  {
    fprintf (stderr, "Error %d: %s\n", error, description);
  }
}

int main (int, char**)
{
  // Setup window
  glfwSetErrorCallback (errorCallback);

  if (!glfwInit())
  {
    return 1;
  }

  // Load CSG-tree
  auto aExtTrsf = Eigen::Scaling (1.0f) * Eigen::AngleAxisf (M_PI * 0.5f, Vec3f (1.f, 0.f, 0.f));
  Eigen::Affine3f aSphereTransform = aExtTrsf * Eigen::Translation3f (0.f, 0.4f, 0.f) * Eigen::Scaling (0.35f);
  Eigen::Affine3f aBoxTransform;
  aBoxTransform = aExtTrsf * Eigen::Translation3f (0.f, -0.1f, 0.f) * Eigen::Scaling (0.5f, 0.2f, 0.4f);

  // std::unique_ptr<CsgNode> aTree (new CsgOperationNode (CSG_OP_UNION,
  //   new CsgPrimitiveNode (CSG_SPHERE, aSphereTransform.matrix()),
  //   new CsgPrimitiveNode (CSG_BOX, aBoxTransform.matrix())));

  json11::Json aData = csg::Parser::parse ("sample_cubes.csg");

  std::unique_ptr<CsgNode> aTree (CsgLoader::LoadTree (aData));

  aTree->InitializeBounds();

  // Init distance field
  VoxelData aDistanceFiled (84, 84, 84,
                            aTree->Bounds().CornerMin(),
                            aTree->Bounds().CornerMax());

  const float aMinPointX = aDistanceFiled.MinCorner.x() + 0.5f * aDistanceFiled.CellSize.x();
  const float aMinPointY = aDistanceFiled.MinCorner.y() + 0.5f * aDistanceFiled.CellSize.y();
  const float aMinPointZ = aDistanceFiled.MinCorner.z() + 0.5f * aDistanceFiled.CellSize.z();

  std::cout << aDistanceFiled.MinCorner.transpose() << std::endl;
  std::cout << aDistanceFiled.MaxCorner.transpose() << std::endl;

  Vec4f aQuery (0.f, 0.f, 0.f, 1.f);

  for (int aX = 0; aX < aDistanceFiled.SizeX; ++aX)
  {
    aQuery[0] = aMinPointX + aX * aDistanceFiled.CellSize.x();

    for (int aY = 0; aY < aDistanceFiled.SizeY; ++aY)
    {
      aQuery[1] = aMinPointY + aY * aDistanceFiled.CellSize.y();

      for (int aZ = 0; aZ < aDistanceFiled.SizeZ; ++aZ)
      {
        aQuery[2] = aMinPointZ + aZ * aDistanceFiled.CellSize.z();

        aDistanceFiled.Value (aX, aY, aZ) = evalDistance (aQuery, aTree.get());
      }
    }
  }

  // Setup window
  GLFWwindow* aWindow = glfwCreateWindow (1280, 720, "csgviewer", NULL, NULL);
  glfwMakeContextCurrent (aWindow);

  // Setup ImGui
  ImGui_ImplGlfw_Init (aWindow, true);

  bool show_test_window = false;
  ImVec4 clear_color = ImColor (114, 144, 154);

  ShaderProgram aProgram (vs_str, fs_str);

  if (!aProgram.infoLog().empty())
  {
    std::cout << aProgram.infoLog() << std::endl;
  }

  // Init 1-component 3D texture
  Texture3D aDistFieldTexture (1);

  aDistFieldTexture.SetFiltering (TextureFilterMode (GL_LINEAR, GL_LINEAR));

  if (!aDistFieldTexture.Init (aDistanceFiled.SizeX,
                               aDistanceFiled.SizeY,
                               aDistanceFiled.SizeZ,
                               aDistanceFiled.Data))
  {
    return 1;
  }

  const float aSceneRadius = (aDistanceFiled.MaxCorner - aDistanceFiled.MinCorner).norm();
  float aSceneEpsilon = aSceneRadius * 1e-3f;
  float aCameraAngle = 0.f;
  float aCameraDistance = aSceneRadius;
  float aCamDistFactor = 1.f;
  double aLastMouseX = -1.0, aLastMouseY = -1.0;

  // Setup screen-quad VBO
  GLuint aScreenQuadVbo;
  glGenBuffers (1, &aScreenQuadVbo);
  glBindBuffer (GL_ARRAY_BUFFER, aScreenQuadVbo);
  glBufferData (GL_ARRAY_BUFFER, sizeof (GLfloat) * 3 * 6, aQuadVertices, GL_STATIC_DRAW);
  glBindBuffer (GL_ARRAY_BUFFER, 0);

  // Main loop
  while (!glfwWindowShouldClose (aWindow))
  {
    glfwPollEvents();
    ImGui_ImplGlfw_NewFrame();

    ImGuiIO& io = ImGui::GetIO();

    if (!io.WantCaptureMouse)
    {
      if (io.MouseDown[0])
      {
        double aMouseX, aMouseY;
        glfwGetCursorPos (aWindow, &aMouseX, &aMouseY);

        if (aLastMouseX >= 0.0)
        {
          aCameraAngle += (aMouseX - aLastMouseX) * 0.03f;
        }

        aLastMouseX = aMouseX;
        aLastMouseY = aMouseY;
      }
      else
      {
        aLastMouseX = -1.0;
      }
    }

    {
      if (ImGui::Button ("Test Window"))
      {
        show_test_window ^= 1;
      }

      ImGui::DragFloat ("Scene epsilon", &aSceneEpsilon, 1e-4f);

      // Camera angle slider
      ImGui::DragFloat ("Camera angle", &aCameraAngle, 0.0f, 3.141592f);

      ImGui::SliderFloat ("Camera zoom", &aCamDistFactor, 0.1f, 3.f);
      ImGui::Text ("Camera distance: %f", aCameraDistance * aCamDistFactor);

      ImGui::Text ("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);
    }

    if (show_test_window)
    {
      ImGui::SetNextWindowPos (ImVec2 (650, 20), ImGuiSetCond_FirstUseEver);
      ImGui::ShowTestWindow (&show_test_window);
    }

    // Rendering
    int aDisplayWidth, aDisplayHeight;
    glfwGetFramebufferSize (aWindow, &aDisplayWidth, &aDisplayHeight);
    glViewport (0, 0, aDisplayWidth, aDisplayHeight);
    glClear (GL_COLOR_BUFFER_BIT);

    // Draw screen quad
    glUseProgram (aProgram.programId());

    // Set uniforms
    glUniform3f (glGetUniformLocation (aProgram.programId(), "iResolution"), aDisplayWidth, aDisplayHeight, 0.f);
    glUniform1f (glGetUniformLocation (aProgram.programId(), "iGlobalTime"), glfwGetTime());
    glUniform1f (glGetUniformLocation (aProgram.programId(), "iCameraAngle"), aCameraAngle);
    glUniform1f (glGetUniformLocation (aProgram.programId(), "iCameraDistance"), aCameraDistance * aCamDistFactor);
    glUniform1ui (glGetUniformLocation (aProgram.programId(), "iDistanceTexture"), 0u);
    glUniform3fv (glGetUniformLocation (aProgram.programId(), "iBoundsMin"), 1, reinterpret_cast<GLfloat*> (&aDistanceFiled.MinCorner));
    glUniform3fv (glGetUniformLocation (aProgram.programId(), "iBoundsMax"), 1, reinterpret_cast<GLfloat*> (&aDistanceFiled.MaxCorner));
    glUniform1f (glGetUniformLocation (aProgram.programId(), "iSceneRadius"), aSceneRadius);
    glUniform1f (glGetUniformLocation (aProgram.programId(), "iSceneEpsilon"), aSceneEpsilon);

    aDistFieldTexture.Bind (GL_TEXTURE0);
    glBindBuffer (GL_ARRAY_BUFFER, aScreenQuadVbo);
    glEnableClientState (GL_VERTEX_ARRAY);
    glVertexPointer (3, GL_FLOAT, 0, NULL);
    glDrawArrays (GL_TRIANGLES, 0, 6);
    glDisableClientState (GL_VERTEX_ARRAY);
    glBindBuffer (GL_ARRAY_BUFFER, 0);
    aDistFieldTexture.Unbind (GL_TEXTURE0);
    glUseProgram (0);

    // Draw ImGui stuff
    glActiveTexture (GL_TEXTURE0);
    ImGui::Render();
    glfwSwapBuffers (aWindow);
  }

  glDeleteBuffers (1, &aScreenQuadVbo);

  // Cleanup
  ImGui_ImplGlfw_Shutdown();
  glfwTerminate();

  return 0;
}


/*

Copyright (c) 2005-2017, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef STOCHASTICWNTCELLCYCLEMODEL_HPP_
#define STOCHASTICWNTCELLCYCLEMODEL_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "WntCellCycleModel.hpp"
#include "RandomNumberGenerator.hpp"

/**
 * Wnt-dependent cell-cycle model with a stochastic G2 duration.
 *
 * Note that this class uses C++'s default copying semantics, and so
 * doesn't implement a copy constructor or operator=.
 */
class StochasticWntCellCycleModel : public WntCellCycleModel
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<WntCellCycleModel>(*this);

        // Make sure the random number generator is also archived
        SerializableSingleton<RandomNumberGenerator>* p_rng_wrapper = RandomNumberGenerator::Instance()->GetSerializationWrapper();
        archive & p_rng_wrapper;

        archive & mStochasticG2Duration;
    }

    /** The duration of the G2 phase, set stochastically. */
    double mStochasticG2Duration;

    /**
     * This method introduces the stochastic element of this class.
     *
     * We allow the duration of the G2 phase of the cell cycle to
     * vary as a normal random deviate with a mean of its deterministic
     * duration, a standard deviation of 0.9 hours, and a cutoff to
     * ensure that it is greater than some minimum value.
     *
     */
    void GenerateStochasticG2Duration();

protected:

    /**
     * Protected copy-constructor for use by CreateCellCycleModel.
     * The only way for external code to create a copy of a cell cycle model
     * is by calling that method, to ensure that a model of the correct subclass is created.
     * This copy-constructor helps subclasses to ensure that all member variables are correctly copied when this happens.
     *
     * This method is called by child classes to set member variables for a daughter cell upon cell division.
     * Note that the parent cell cycle model will have had ResetForDivision() called just before CreateCellCycleModel() is called,
     * so performing an exact copy of the parent is suitable behaviour. Any daughter-cell-specific initialisation
     * can be done in InitialiseDaughterCell().
     *
     * @param rModel the cell cycle model to copy.
     */
    StochasticWntCellCycleModel(const StochasticWntCellCycleModel& rModel);

public:
    /**
     * The standard constructor called in tests.
     *
     * @param pOdeSolver An optional pointer to a cell-cycle model ODE solver object (allows the use of different ODE solvers)
     */
    StochasticWntCellCycleModel(boost::shared_ptr<AbstractCellCycleModelOdeSolver> pOdeSolver = boost::shared_ptr<AbstractCellCycleModelOdeSolver>());

    /** Empty virtual destructor so archiving works with static libraries. */
    virtual ~StochasticWntCellCycleModel();

    /**
     * Overridden builder method to create new copies of
     * this cell-cycle model.
     *
     * @return the new cell-cycle model
     */
    AbstractCellCycleModel* CreateCellCycleModel();

    /**
     * Set the duration of the G2 phase for the daughter cell.
     */
    void InitialiseDaughterCell();

    /**
     * Initialise the cell-cycle model at the start of a simulation.
     *
     * This overridden method sets up a new WntCellCycleOdeSystem,
     * sets the cell type according to the current beta catenin level
     * and sets a random G2 duration.
     */
    void Initialise();

    /**
     * Reset cell-cycle model by calling AbstractOdeBasedPhaseBasedCellCycleModel::ResetForDivision()
     * and setting a new random G2 duration.
     */
    void ResetForDivision();

    /**
     * @return the duration of the G2 phase.
     */
    double GetG2Duration() const;

    /**
     * Overridden OutputCellCycleModelParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    virtual void OutputCellCycleModelParameters(out_stream& rParamsFile);
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(StochasticWntCellCycleModel)
#include "CellCycleModelOdeSolverExportWrapper.hpp"
EXPORT_CELL_CYCLE_MODEL_ODE_SOLVER(StochasticWntCellCycleModel)

#endif /*STOCHASTICWNTCELLCYCLEMODEL_HPP_*/


/*
 * The MIT License
 *
 * Copyright (c) 2012-2018 The University of Utah
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include "ConvectiveInterpolationMethods.h"

#include <boost/bimap.hpp>

#include <stdexcept>
#include <sstream>

namespace WasatchCore {

  typedef boost::bimap<std::string,ConvInterpMethods> ConvInterpStringMap;
  static ConvInterpStringMap validConvInterpStrings;

  void set_conv_interp_string_map()
  {
    if( !validConvInterpStrings.empty() ) return;
    typedef ConvInterpStringMap::left_value_type LVT;
    validConvInterpStrings.left.insert( LVT("CENTRAL" , CENTRAL ) );
    validConvInterpStrings.left.insert( LVT("UPWIND"  , UPWIND  ) );
    validConvInterpStrings.left.insert( LVT("SUPERBEE", SUPERBEE) );
    validConvInterpStrings.left.insert( LVT("CHARM"   , CHARM   ) );
    validConvInterpStrings.left.insert( LVT("KOREN"   , KOREN   ) );
    validConvInterpStrings.left.insert( LVT("MC"      , MC      ) );
    validConvInterpStrings.left.insert( LVT("OSPRE"   , OSPRE   ) );
    validConvInterpStrings.left.insert( LVT("SMART"   , SMART   ) );
    validConvInterpStrings.left.insert( LVT("VANLEER" , VANLEER ) );
    validConvInterpStrings.left.insert( LVT("HCUS"    , HCUS    ) );
    validConvInterpStrings.left.insert( LVT("MINMOD"  , MINMOD  ) );
    validConvInterpStrings.left.insert( LVT("HQUICK"  , HQUICK  ) );
  }

  //------------------------------------------------------------------

  ConvInterpMethods get_conv_interp_method( std::string key )
  {
    set_conv_interp_string_map();
    std::transform( key.begin(), key.end(), key.begin(), ::toupper );
    ConvInterpStringMap::left_const_iterator ii = validConvInterpStrings.left.find(key);
    if( ii == validConvInterpStrings.left.end() ){
      std::ostringstream msg;
      msg << __FILE__ << " : " << __LINE__ << std::endl
          << "No matching upwind method for '" << key << "'" << std::endl;
    }
    return ii->second;
  }

  std::string get_conv_interp_method( const ConvInterpMethods key )
  {
    set_conv_interp_string_map();
    return validConvInterpStrings.right.find(key)->second;
  }

} // namespace WasatchCore


//=======================================================================
// Copyright (c) Aaron Windsor 2007
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef __PLANAR_CANONICAL_ORDERING_HPP__
#define __PLANAR_CANONICAL_ORDERING_HPP__

#include <vector>
#include <list>
#include <boost/config.hpp>
#include <boost/next_prior.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost
{

namespace detail
{
    enum planar_canonical_ordering_state
    {
        PCO_PROCESSED,
        PCO_UNPROCESSED,
        PCO_ONE_NEIGHBOR_PROCESSED,
        PCO_READY_TO_BE_PROCESSED
    };
}

template < typename Graph, typename PlanarEmbedding, typename OutputIterator,
    typename VertexIndexMap >
void planar_canonical_ordering(const Graph& g, PlanarEmbedding embedding,
    OutputIterator ordering, VertexIndexMap vm)
{

    typedef typename graph_traits< Graph >::vertex_descriptor vertex_t;
    typedef typename graph_traits< Graph >::edge_descriptor edge_t;
    typedef
        typename graph_traits< Graph >::adjacency_iterator adjacency_iterator_t;
    typedef typename property_traits< PlanarEmbedding >::value_type
        embedding_value_t;
    typedef typename embedding_value_t::const_iterator embedding_iterator_t;
    typedef iterator_property_map< typename std::vector< vertex_t >::iterator,
        VertexIndexMap >
        vertex_to_vertex_map_t;
    typedef iterator_property_map<
        typename std::vector< std::size_t >::iterator, VertexIndexMap >
        vertex_to_size_t_map_t;

    std::vector< vertex_t > processed_neighbor_vector(num_vertices(g));
    vertex_to_vertex_map_t processed_neighbor(
        processed_neighbor_vector.begin(), vm);

    std::vector< std::size_t > status_vector(
        num_vertices(g), detail::PCO_UNPROCESSED);
    vertex_to_size_t_map_t status(status_vector.begin(), vm);

    std::list< vertex_t > ready_to_be_processed;

    vertex_t first_vertex = *vertices(g).first;
    vertex_t second_vertex = first_vertex;
    adjacency_iterator_t ai, ai_end;
    for (boost::tie(ai, ai_end) = adjacent_vertices(first_vertex, g);
         ai != ai_end; ++ai)
    {
        if (*ai == first_vertex)
            continue;
        second_vertex = *ai;
        break;
    }

    ready_to_be_processed.push_back(first_vertex);
    status[first_vertex] = detail::PCO_READY_TO_BE_PROCESSED;
    ready_to_be_processed.push_back(second_vertex);
    status[second_vertex] = detail::PCO_READY_TO_BE_PROCESSED;

    while (!ready_to_be_processed.empty())
    {
        vertex_t u = ready_to_be_processed.front();
        ready_to_be_processed.pop_front();

        if (status[u] != detail::PCO_READY_TO_BE_PROCESSED
            && u != second_vertex)
            continue;

        embedding_iterator_t ei, ei_start, ei_end;
        embedding_iterator_t next_edge_itr, prior_edge_itr;

        ei_start = embedding[u].begin();
        ei_end = embedding[u].end();
        prior_edge_itr = prior(ei_end);
        while (source(*prior_edge_itr, g) == target(*prior_edge_itr, g))
            prior_edge_itr = prior(prior_edge_itr);

        for (ei = ei_start; ei != ei_end; ++ei)
        {

            edge_t e(*ei); // e = (u,v)
            next_edge_itr
                = boost::next(ei) == ei_end ? ei_start : boost::next(ei);
            vertex_t v = source(e, g) == u ? target(e, g) : source(e, g);

            vertex_t prior_vertex = source(*prior_edge_itr, g) == u
                ? target(*prior_edge_itr, g)
                : source(*prior_edge_itr, g);
            vertex_t next_vertex = source(*next_edge_itr, g) == u
                ? target(*next_edge_itr, g)
                : source(*next_edge_itr, g);

            // Need prior_vertex, u, v, and next_vertex to all be
            // distinct. This is possible, since the input graph is
            // triangulated. It'll be true all the time in a simple
            // graph, but loops and parallel edges cause some complications.
            if (prior_vertex == v || prior_vertex == u)
            {
                prior_edge_itr = ei;
                continue;
            }

            // Skip any self-loops
            if (u == v)
                continue;

            // Move next_edge_itr (and next_vertex) forwards
            // past any loops or parallel edges
            while (next_vertex == v || next_vertex == u)
            {
                next_edge_itr = boost::next(next_edge_itr) == ei_end
                    ? ei_start
                    : boost::next(next_edge_itr);
                next_vertex = source(*next_edge_itr, g) == u
                    ? target(*next_edge_itr, g)
                    : source(*next_edge_itr, g);
            }

            if (status[v] == detail::PCO_UNPROCESSED)
            {
                status[v] = detail::PCO_ONE_NEIGHBOR_PROCESSED;
                processed_neighbor[v] = u;
            }
            else if (status[v] == detail::PCO_ONE_NEIGHBOR_PROCESSED)
            {
                vertex_t x = processed_neighbor[v];
                // are edges (v,u) and (v,x) adjacent in the planar
                // embedding? if so, set status[v] = 1. otherwise, set
                // status[v] = 2.

                if ((next_vertex == x
                        && !(first_vertex == u && second_vertex == x))
                    || (prior_vertex == x
                        && !(first_vertex == x && second_vertex == u)))
                {
                    status[v] = detail::PCO_READY_TO_BE_PROCESSED;
                }
                else
                {
                    status[v] = detail::PCO_READY_TO_BE_PROCESSED + 1;
                }
            }
            else if (status[v] > detail::PCO_ONE_NEIGHBOR_PROCESSED)
            {
                // check the two edges before and after (v,u) in the planar
                // embedding, and update status[v] accordingly

                bool processed_before = false;
                if (status[prior_vertex] == detail::PCO_PROCESSED)
                    processed_before = true;

                bool processed_after = false;
                if (status[next_vertex] == detail::PCO_PROCESSED)
                    processed_after = true;

                if (!processed_before && !processed_after)
                    ++status[v];

                else if (processed_before && processed_after)
                    --status[v];
            }

            if (status[v] == detail::PCO_READY_TO_BE_PROCESSED)
                ready_to_be_processed.push_back(v);

            prior_edge_itr = ei;
        }

        status[u] = detail::PCO_PROCESSED;
        *ordering = u;
        ++ordering;
    }
}

template < typename Graph, typename PlanarEmbedding, typename OutputIterator >
void planar_canonical_ordering(
    const Graph& g, PlanarEmbedding embedding, OutputIterator ordering)
{
    planar_canonical_ordering(g, embedding, ordering, get(vertex_index, g));
}

} // namespace boost

#endif //__PLANAR_CANONICAL_ORDERING_HPP__