// Copyright 2021 National Technology & Engineering Solutions of Sandia, LLC (NTESS). Under the terms of Contract DE-NA0003525 with NTESS, the U.S. Government retains certain rights in this software. See LICENSE.txt for details.
//
// Author: Denis Bueno

// Represented by HornTransStateSpace: What is common to all the sliced
// transition relations? The state variables of the transition system; the rule
// activation vars.
//
// Represented by HornSetTrans: What is different between the sliced transition
// relations? The transition relation itself. The inputs derived from the
// quantified vars. The inputs for the commander vars. The cones.

#define BOOST_RESULT_OF_USE_DECLTYPE

#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/combine.hpp>
#include <fstream>
#include <gmpxx.h>
#include <map>
#include <set>
#include <sstream>
#include <vector>
#include <z3++.h>

#include "checker_types.h"
#include "supp/expr_flattener.h"
#include "supp/expr_iterators.h"
#include "supp/expr_rewriter.h"
#include "supp/expr_substitution.h"
#include "supp/expr_supp.h"
#include "supp/nnf_rewriter.h"
#include "xsys/state_vars.h"
#include "xsys/vmt_transition_system.h"
#include "one_hot.h"

using namespace std;
using namespace euforia;

using euforia::vmt::VmtTransitionSystem;

namespace {
struct Config {
  string input_filename;
  bool allocate_vars_by_sort = false;
  bool allocate_inputs_by_sort = true;
  boost::optional<string> varmap_filename;
  bool slice_t = false;
  bool enable_one_hots = true;
};

Config config;

class Int2BvRewriter : public ExprRewriter<Int2BvRewriter>,
    public ExprVisitor<Int2BvRewriter, z3::expr> {
 public:
  Int2BvRewriter(z3::context& c, int bv_width) : bv_sort_(c.bv_sort(bv_width)) {}

  z3::expr operator()(const z3::expr& e) { return Rewrite(e); }

  z3::sort TranslateSort(const z3::sort& s) {
    switch (s.sort_kind()) {
      case Z3_INT_SORT:
        return bv_sort_;
      case Z3_ARRAY_SORT:
        return ctx().array_sort(TranslateSort(s.array_domain()),
                                TranslateSort(s.array_range()));
      default:
        return s;
    }
  }

  z3::expr visitExpr(const z3::expr& e) {
    return e.decl()(NewArgsExprVector(e));
  }

  z3::expr visitQUANTIFIER(const z3::expr& e) {
    z3::expr e_new(e.ctx());
    vector<Z3_sort> var_sorts;
    vector<Z3_symbol> var_names;
    for (unsigned i = 0; i < Z3_get_quantifier_num_bound(e.ctx(), e); i++) {
      z3::sort var_sort(e.ctx(), Z3_get_quantifier_bound_sort(e.ctx(), e, i));
      var_sorts.push_back(TranslateSort(var_sort));
      var_names.push_back(Z3_get_quantifier_bound_name(e.ctx(), e, i));
    }
    if (e.is_forall()) {
      Z3_pattern pats[0];
      e_new = z3::to_expr(
          e.ctx(),
          Z3_mk_forall(e.ctx(), 0, 0, pats, var_sorts.size(),
                       var_sorts.data(), var_names.data(), lookup(e.body())));
    } else if (e.is_exists()) {
      Z3_pattern pats[0];
      e_new = z3::to_expr(
          e.ctx(),
          Z3_mk_exists(e.ctx(), 0, 0, pats, var_sorts.size(),
                       var_sorts.data(), var_names.data(), lookup(e.body())));
    } else {
      EUFORIA_FATAL("unknown quantifier");
    }
    // logger.Log(0, "{}:{}", e, e.get_sort());
    // logger.Log(0, "{}:{}", e_new, e_new.get_sort());
    return e_new;
  }

  z3::expr visitVAR(const z3::expr& e) {
    auto ret = z3::to_expr(
        e.ctx(),
        Z3_mk_bound(e.ctx(), Z3_get_index_value(e.ctx(), e),
                    TranslateSort(e.get_sort())));
    return ret;
  }

  z3::expr visitANUM(const z3::expr& e) {
    string decimal_str;
    z3::expr bv(e.ctx());
    if (!e.is_numeral(decimal_str)) {
      EUFORIA_FATAL("not a numeral: {}", e);
    }
    logger.Log(4, "found numeral: {}", decimal_str);
    mpz_class num(decimal_str);
    string binstr = num.get_str(2);
    logger.Log(4, "    binary: {}", binstr);
    // create bits, which has num but padded with zeros
    string bits(bv_sort_.bv_size(), '0');
    int width = static_cast<int>(bv_sort_.bv_size());
    // binstr will be bits if num is positive, and -bits if num is negative
    if (binstr[0] != '-') {
      assert(binstr.size() > 0);
      assert(binstr.size() <= bv_sort_.bv_size());
      copy(binstr.begin(), binstr.end(),
           bits.begin() + (bits.size() - binstr.size()));
      bool bool_bits[width];
      // bits is in reverse order from what z3 expects; z3 wants LSB first
      for (int i = 0, j = width-1; i < width; i++, j--) {
        bool_bits[j] = bits[i] == '0' ? false : true;
      }
      bv = ctx().bv_val(width, bool_bits);
      logger.Log(4, "    binary padded: {}", bits);
    } else {
      assert(binstr.size() > 1);
      // create bits, which has num but padded with zeros
      copy(next(binstr.begin()), binstr.end(),
           next(bits.begin()) + (bits.size() - binstr.size()));
      logger.Log(4, "    binary uminus: {}", bits);
      bool bool_bits[width];
      // bits is in reverse order from what z3 expects; z3 wants LSB first
      for (int i = 0, j = width-1; i < width; i++, j--) {
        bool_bits[j] = bits[i] == '0' ? false : true;
      }
      bv = ctx().bv_val(width, bool_bits);
      // constant 1 bits
      for (int i = 0; i < width; i++) {
        bool_bits[i] = true;
      }
      auto all_ones = ctx().bv_val(width, bool_bits);
      // flip all the bits and add one: 2's complement representation
      bv = bv ^ all_ones;
      bv = bv + 1;
      bv = bv.simplify();
    }
    logger.Log(4, "    bv result: {}", bv);
    return bv;
  }

  z3::expr visitUNINTERPRETED(const z3::expr& e) {
    assert(!e.is_array());
    // Create an uninterpreted function that takes BVs instead of Ints
    auto e_decl = e.decl();
    vector<z3::sort> f_sorts;
    for (unsigned i = 0; i < e_decl.arity(); i++) {
      f_sorts.push_back(TranslateSort(e_decl.domain(i)));
    }
    auto f_decl = e.ctx().function(
        e_decl.name(), e_decl.arity(), f_sorts.data(),
        TranslateSort(e_decl.range()));
    return f_decl(NewArgsExprVector(e));
  }

  z3::expr visitSELECT(const z3::expr& e) {
    logger.Log(4, "select {}", e);
    for (auto&& arg : boost::make_iterator_range(args_begin(e), args_end(e))) {
      logger.Log(4, "    rewritten args {}:{}", arg, arg.get_sort());
    }
    return z3::select(arg(e,0), arg(e,1));
  }

  z3::expr visitSTORE(const z3::expr& e) {
    return z3::store(arg(e,0), arg(e,1), arg(e,2));
  }

  z3::expr visitEQ(const z3::expr& e) {
    return arg(e,0) == arg(e,1);
  }

  z3::expr visitADD(const z3::expr& e) {
    z3::expr ret(e.ctx());
    for (auto&& arg : boost::make_iterator_range(args_begin(e), args_end(e))) {
      ret = bool(ret) ? ret + arg : arg;
    }
    return ret;
  }

  z3::expr visitSUB(const z3::expr& e) {
    assert(e.num_args() == 2);
    return arg(e,0) - arg(e,1);
  }

  z3::expr visitUMINUS(const z3::expr& e) {
    return -arg(e,0);
  }

  z3::expr visitMUL(const z3::expr& e) {
    assert(e.num_args() == 2);
    return arg(e,0) * arg(e,1);
  }

  z3::expr visitDIV(const z3::expr& e) {
    return arg(e,0) / arg(e,1); // signed division
  }

  z3::expr visitIDIV(const z3::expr& e) {
    return arg(e,0) / arg(e,1);
  }

  z3::expr visitREM(const z3::expr& e) {
    return z3::srem(arg(e,0), arg(e,1));
  }

  z3::expr visitMOD(const z3::expr& e) {
    return z3::smod(arg(e,0), arg(e,1));
  }

  z3::expr visitLE(const z3::expr& e) {
    return arg(e,0) <= arg(e,1);
  }

  z3::expr visitLT(const z3::expr& e) {
    return arg(e,0) < arg(e,1);
  }

  z3::expr visitGE(const z3::expr& e) {
    return arg(e,0) >= arg(e,1);
  }

  z3::expr visitGT(const z3::expr& e) {
    return arg(e,0) > arg(e,1);
  }

  z3::context& ctx() const { return bv_sort_.ctx(); }

 private:
  z3::sort bv_sort_;
};

//^----------------------------------------------------------------------------^
//

class HornRule {
 public:
  HornRule(const z3::expr& rule, int index)
      : head_(rule.ctx()), body_(rule.ctx()), rule_(rule), index_(index) {
    auto b = rule;
    if (rule.is_quantifier()) {
      b = rule.body();
    } else if (is_not(rule) && rule.arg(0).is_quantifier()) {
      b = rule.arg(0).body();
    }
    body_ = ctx().bool_val(true);
    if (b.is_implies()) {
      head_ = b.arg(1);
      body_ = b.arg(0);
    } else {
      head_ = b;
    }
  }

  bool is_linear() const {
    IterExprSet visited;
    int relation_count =
        count_if(df_expr_ext_iterator::begin(body(), visited),
                 df_expr_ext_iterator::end(body(), visited),
                 [&](auto&& x) { return x.is_app() &&
                     x.decl().decl_kind() == Z3_OP_UNINTERPRETED; });
    // int relation_count =
    //     count_if(df_expr_iterator::begin(rule.body()),
    //              df_expr_iterator::end(rule.body()),
    //              [&](auto&& x) { return x.is_app() &&
    //                  relation_vars_.find(x.decl()) != relation_vars_.end(); });
    if (relation_count > 1) {
      return false;
      // throw std::runtime_error("nonlinear Horn clause detected, exiting");
    }
    return true;
  }

  bool is_query() const {
    if (is_literal_false(head())) {
      return true;
    }
   if (is_not(head()) &&
       head().arg(0).decl().decl_kind() == Z3_OP_UNINTERPRETED) {
     return true;
   }
   return false;
  }

  int index() const { return index_; }
  z3::context& ctx() const { return head_.ctx(); }
  z3::expr head() const { return head_; }
  z3::expr body() const { return body_; }
  operator z3::expr() const { return rule_; }
  std::string head_name() const {
    return head_.decl().name().str();
  }

 private:
  z3::expr head_;
  z3::expr body_;
  z3::expr rule_;
  int index_;
};

std::ostream& operator<<(std::ostream& os, const HornRule& r) {
  fmt::print(os, "HornRule: head {} body:\n{}", r.head(), r.body());
  return os;
}

//^----------------------------------------------------------------------------^
//

//! Groups a set of Horn clauses all of which have the same head symbol. The
//! head symbol is determined by the first rule added to the group.
class HornTransition {
 public:
  HornTransition() {}

  void AddRule(const HornRule& r, int i) {
    if (head_name_) {
      if (r.head_name() != head_name()) {
        throw std::runtime_error(
            fmt::format("adding bad rule to {} group: {}", *head_name_, r));
      }
    } else {
      head_name_ = r.head_name();
    }
    rules_.push_back({r,i});
  }

  const std::string& head_name() const { return *head_name_; }
  const std::vector<HornRule>& rules() const { return rules_; }

 private:
  boost::optional<std::string> head_name_;
  std::vector<HornRule> rules_;
};

//^----------------------------------------------------------------------------^
//

// Represents a complete set of Horn clauses -- a model checking instance -- as
// a collection of rules. Supports parsing both SMT2-style and
// declare-rel-style Horn clauses, thanks to z3.
class HornClauses {
 public:
  HornClauses(z3::context& c, const char *filename, bool int2bv = false)
      : ctx_(c) {

    // Reads file into buffer because we may need to parse twice
    std::ifstream fin(filename);
    std::stringstream sstr;
    sstr << fin.rdbuf();
    auto filebuf = sstr.str();

    // Parses as declare-rel at first
    z3::fixedpoint fp(ctx());
    auto vec = Z3_fixedpoint_from_string(c, fp, filebuf.c_str());
    if (!vec) {
      fmt::print(cerr, "Parse error: {}", filename);
      exit(1);
    }
    auto queries = z3::ast_vector_tpl<z3::expr>(c, vec);
    z3::expr_vector rules(c);
    rules = fp.rules();

    if (queries.size() > 0) {
      assert(queries[0].num_args() == 0);
      rules.push_back(z3::implies(queries[0], ctx().bool_val(false)));
    }

    if (rules.empty()) {
      logger.Log(2, "0 fp rules found, so parsing as smtlib2");
      // Parses again as SMTLIB-2 HORN file
      rules = ctx().parse_string(filebuf.c_str());
    }
    logger.Log(1, "parsed {} rules", rules.size());
    if (int2bv) {
      Int2BvRewriter rewrite_int2bv(ctx(), 32);
      for (unsigned i = 0; i < rules.size(); i++) {
        auto r = rewrite_int2bv(rules[i]);
        rules.set(i, r);
      }
    }
    rules_.reserve(rules.size());
    for (unsigned i = 0; i < rules.size(); i++) {
      rules_.push_back(HornRule(rules[i], i));
    }
  }

  z3::context& ctx() const { return ctx_; }

  // Iterates over the database as HornRules
  auto rules() const {
    return boost::make_iterator_range(rules_.begin(), rules_.end());
  }

  FuncDeclMap<HornTransition> group_by_heads() const {
    using namespace boost::adaptors; // indexed
    // Groups horn rules by head symbol
    FuncDeclMap<HornTransition> head2trans;
    for (const auto& indexed_rule : rules() | indexed(0)) {
      const auto& index = indexed_rule.index();
      const auto& rule = indexed_rule.value();
      head2trans[rule.head().decl()].AddRule(rule, index);
    }
    return head2trans;
  }

  size_t size() const { return rules_.size(); }

 private:
  z3::context& ctx_;
  // Contains all the rules plus the query written as a rule (false => head)
  std::vector<HornRule> rules_;
};

//^----------------------------------------------------------------------------^
//

using RelationVarMap = FuncDeclMap<z3::expr>;
using PlaceVarsMap = FuncDeclMap<vector<z3::expr>>;

static string SortName(const z3::sort& sort) {
  if (sort.is_bv()) {
    return fmt::format("bv{}", sort.bv_size());
  } else if (sort.is_bool()) {
    return "bool";
  } else if (sort.is_array()) {
    auto domain_name = SortName(sort.array_domain());
    auto range_name = SortName(sort.array_range());
    return fmt::format("arr-{}-{}", domain_name, range_name);
  } else {
    fmt::print("{}\n", sort);
    EUFORIA_FATAL("unhandled sort");
  }
}

//! Represents the state space for translating a set of Horn clauses. Creates
//! relation variables, place variables, and rule activation inputs. All such
//! objects are created in the current output transition system, set in the
//! constructor and via set_output_xsys.
//!
//! This can be used to represent the state space for an entire set of clauses.
//! But it also has the flexibility of representing the state space for a
//! subset of clauses.
class HornTransStateSpace {
 public:
  HornTransStateSpace() : xsys_(nullptr) {}
  HornTransStateSpace(Config& cfg, const std::vector<HornRule>& rules,
                      VmtTransitionSystem *xsys)
      : xsys_(xsys) {
    SortMap<std::vector<z3::expr>> relation_places_by_sort;
    for (const auto& rule : rules) {
      logger.Log(3, "creating state space for rule: {} => {}", rule.body(),
                 rule.head());
      // Creates rule activation variables
      auto rule_activation_input =
          ctx().bool_const(("r-" + to_string(rule.index())).c_str());
      auto& rule_activation = xsys_->AddInput(MakeInput(rule_activation_input));
      activation_input_vars_.push_back(rule_activation);
      // Pick up relations from rule heads; then declare Boolean relation vars
      // and place vars for each place.
      if (auto decl = rule.head().decl();
          decl.decl_kind() == Z3_OP_UNINTERPRETED) {
        // Create r elation var
        auto relation_name = decl.name().str();
        auto var_name = "at-" + relation_name;
        if (xsys_->FindVar(var_name)) { // Skips present variables.
          continue;
        }
        auto& var = xsys_->AddVar(MakeStateVar(
                ctx().bool_const(var_name.c_str()),
                ctx().bool_const((var_name + "+").c_str())));
        relation_vars_.insert({decl, var.current()});
        // Ensures vector is initialized for all relations, even nullary ones
        (void)place_vars_[decl];
        // Open varmap file, if we need to
        ofstream varmap_out;
        if (cfg.varmap_filename) {
          varmap_out = ofstream(*cfg.varmap_filename);
          if (!varmap_out) {
            fmt::print(cerr, "warning: could not open varmap file: {}:\n{}\n",
                       *cfg.varmap_filename, strerror(errno));
            cfg.varmap_filename = boost::none;
          }
        }
        if (cfg.allocate_vars_by_sort) {
          SortMap<int> counts;
          for (unsigned i = 0; i < decl.arity(); i++) {
            if (counts.find(decl.domain(i)) == counts.end()) {
              counts[decl.domain(i)] = 0;
            }
            counts[decl.domain(i)] += 1;
          }
          for (auto&& [sort, num_occurrences] : counts) {
            int num_places =
                static_cast<int>(relation_places_by_sort[sort].size());
            for (int i = 0; i < (num_occurrences - num_places); i++) {
              auto name =
                  (SortName(sort) + "-place-" + to_string(num_places+i));
              auto& place_var = xsys_->AddVar(MakeStateVar(
                      ctx().constant(name.c_str(), sort),
                      ctx().constant((name + "+").c_str(), sort)));
              relation_places_by_sort[sort].push_back(place_var.current());
            }
          }
          counts.clear();
          for (unsigned i = 0; i < decl.arity(); i++) {
            auto place_var = relation_places_by_sort.at(
                decl.domain(i))[counts[decl.domain(i)]];
            if (varmap_out) {
              fmt::print(varmap_out, "Relation {} place {} mapped to var {}\n",
                         relation_name, i, place_var);
            }
            // EUFORIA_DEBUG_BREAK;
            place_vars_[decl].push_back(place_var);
            counts[decl.domain(i)]++;
          }
        } else {
          // allocate vars by relation
          for (unsigned i = 0; i < decl.arity(); i++) {
            auto place_name = relation_name + "-place-" + to_string(i);
            auto& place_var = xsys_->AddVar(MakeStateVar(
                    ctx().constant(place_name.c_str(), decl.domain(i)),
                    ctx().constant((place_name + "+").c_str(),
                                   decl.domain(i))));
            if (varmap_out) {
              fmt::print(varmap_out, "Relation {} place {} mapped to var {}\n",
                         relation_name, i, place_var.current());
            }
            place_vars_[decl].push_back(place_var.current());
          }
        }
      }
    }
    logger.Log(3, "created {} state variables", xsys_->num_vars());
    if (logger.ShouldLog(4)) {
      for (auto&& v : xsys_->vars()) {
        logger.Log(4, "  {}", v.current());
      }
    }
    logger.Log(3, "created {} inputs", xsys_->num_inputs());
  }

  const StateVar& get_head_relation_var(const HornRule& r) const {
    auto& v_expr = relation_vars_.at(r.head().decl());
    return *xsys_->FindVar(v_expr);
  }

  const PrimaryInput& get_activation_var(const HornRule& r) const {
    auto& i_expr = activation_input_vars_.at(r.index());
    return *xsys_->FindInput(i_expr);
  }

  auto iter_place_vars(const z3::expr& e) const {
    class GetVar {
     public:
      GetVar(const VmtTransitionSystem& xsys) : xsys_(xsys) {}
      const StateVar& operator()(const z3::expr& var) const {
        return *xsys_.FindVar(var);
      }
     private:
      const VmtTransitionSystem& xsys_;
    };
    const auto& place_var_vec = place_vars_.at(e.decl());
    return boost::make_iterator_range(
        boost::make_transform_iterator(begin(place_var_vec),
                                       GetVar(*xsys_)),
        boost::make_transform_iterator(end(place_var_vec),
                                       GetVar(*xsys_)));
  }

  //! Iterates over the place variables for the relation in r's head
  auto iter_head_place_vars(const HornRule& r) const {
    return iter_place_vars(r.head());
  }

  //! Iterates over the Boolean relation vars in the transition system
  auto relation_vars() const {
    class GetVar {
     public:
      GetVar(const VmtTransitionSystem& xsys) : xsys_(xsys) {}
      const StateVar& operator()(const RelationVarMap::value_type& p) const {
        return *xsys_.FindVar(p.second);
      }
      const VmtTransitionSystem& xsys_;
    };
    return boost::make_iterator_range(
        boost::make_transform_iterator(relation_vars_.begin(), GetVar(*xsys_)),
        boost::make_transform_iterator(relation_vars_.end(), GetVar(*xsys_)));
  }

  auto activation_input_vars_begin() const {
    return activation_input_vars_.cbegin();
  }

  auto activation_input_vars_end() const {
    return activation_input_vars_.cend();
  }

  //! Returns zipped iterator over [ <e-arg-0, place-var-0>, <e-arg-1,
  //! place-var-1>, ... ]. Precondition: e must be an unintpreted Horn relation
  //! symbol.
  auto iter_args_with_places(const z3::expr& e) const {
    assert(e.num_args() == place_vars_.at(e.decl()).size());
    return boost::combine(ExprArgs(e), iter_place_vars(e));
  }

  //! Finds Boolean relation var for uninterpreted application e
  boost::optional<z3::expr> find_relation_var(const z3::expr& e) const {
    if (auto search = relation_vars_.find(e.decl());
        search != relation_vars_.end()) {
      return search->second;
    }
    return boost::none;
  }

  z3::context& ctx() const { return xsys_->ctx(); }
  VmtTransitionSystem& xsys() const { return *xsys_; }
  size_t num_relations() const { return relation_vars_.size(); }

  void set_output_xsys(VmtTransitionSystem *xsys) { xsys_ = xsys; }

 private:
  // output to
  VmtTransitionSystem *xsys_;
  // Maps func_decl to Boolean state var
  RelationVarMap relation_vars_;
  // List of Boolean state vars
  std::vector<z3::expr> activation_input_vars_;
  // Maps func_decl to list of place state vars
  PlaceVarsMap place_vars_;
};

//^----------------------------------------------------------------------------^
//
//! Helper for rewriting the bodies of Horn clauses in terms of relation vars
//! and place vars
class Horn2VmtRewriter : public ExprRewriter<Horn2VmtRewriter> {
 public:
  Horn2VmtRewriter(const HornTransStateSpace& space) : space_(space) {}

  z3::expr operator()(const z3::expr& e) { return Rewrite(e); }

  // Precondition: e is a sub-formula of a Horn clause body.
  // If e is a relation occurrence, rewrites it in T-land using the relation
  // and place variables. Otherwise, simply rewrites e using previously
  // rewritten children.
  z3::expr visit(const z3::expr& e) {
    if (e.is_app()) {
      if (auto e_var = space_.find_relation_var(e)) {
        z3::expr_vector conj(e.ctx());
        ExprVectorInserter out(conj);
        *out++ = *e_var;
        for (auto&& zipped : space_.iter_args_with_places(e)) {
          z3::expr arg = boost::get<0>(zipped);
          const StateVar& place_var = boost::get<1>(zipped);
          *out++ = (arg == place_var.current());
        }
        return expr_mk_and(conj);
      }
      return e.decl()(NewArgsExprVector(e));
    }
    return e;
  }

 private:
  const HornTransStateSpace& space_;
};

//^----------------------------------------------------------------------------^
//
//! Represents the translation of a set of Horn clauses into a transition
//! relation. The transition relation is "functional" in that each state
//! variable has exactly one top-level equation for it; the activation vars are
//! also at the top level as implications.
//!
//! Anything head isn't simply H(...) for some uninterpreted H gets put into
//! queries()
class HornSetTrans {
 public:
  HornSetTrans(const HornTransStateSpace& space,
                     const std::vector<HornRule>& rules)
      : trans_(space.ctx()), to_vmt_(space) {
    ExprAndInserter trans_out(trans_);
    CreateQuantifiedVarInputs(space, rules);
    for (const auto& rule : rules) {
      if (rule.head().decl().decl_kind() != Z3_OP_UNINTERPRETED) {
        queries_.push_back(rule);
        continue;
      }
      const auto& activation_var = space.get_activation_var(rule);
      // Translates head and body.
      if (config.allocate_vars_by_sort) {
        // Don't worry about it if we're using the normal allocation strategy
        if (!rule.is_linear()) {
          throw std::runtime_error("nonlinear Horn clause detected, exiting");
        }
      }
      // Fills in var_subst_curr for rule: find var occurrences in relations:
      // 1. if it's a variable, maps [var -> place curr-state variable];
      // 2. otherwise, maps [var -> fresh input]
      ExprSubstitution var_subst_curr = make_var_subst(space, rule);
      // Removes relation occurrences from the rule's body and replaces them
      // with relation vars and place vars
      auto body_condition = to_vmt_(rule.body());
      logger.Log(3, "Rule {} condition from Horn:\n{}", rule.index(),
                 body_condition);
      // Removes bound vars from body_condition
      body_condition = var_subst_curr(body_condition);
      auto rule_condition = z3::implies(activation_var, body_condition);
      logger.Log(2, "Global rule {} condition to emit:\n{}",
                 rule.index(), rule_condition);
      // Done processing body, but it hasn't emitted the rule yet: it'll figure
      // out below which one to emit.

      *trans_out++ = rule_condition; // emit the rule condition

      // Adds this rule's next state to state-update cones
      CreateConesFromRule(space, rule, var_subst_curr);
    }

    // Sends next-state cones, which are complete, to the transition relation
    for (auto it = cones_.begin(), ie = cones_.end(); it != ie;
         // Erases them because they could free a lot of memory
         it = cones_.erase(it)) {
      auto&& [next_var, val] = *it;
      if (next_var.is_bool()) {
        val = val.simplify();
      }
      *trans_out++ = (next_var == val);
    }

    // Adds one-hot constraints for rules: exactly one rule should be chosen on
    // each transition.
    if (config.enable_one_hots) {
      OneHotConstraints ohc(space.ctx());
      ohc.insert(space.activation_input_vars_begin(),
                 space.activation_input_vars_end());
      if (space.activation_input_vars_begin() !=
          space.activation_input_vars_end()) {
        *trans_out++ = ohc.at_most(OneHotConstraints::Config::Commander(3));
        // *trans_out++ = ohc.at_most(OneHotConstraints::Config::Naive());
        for (auto&& v : ohc.commander_vars()) {
          space.xsys().AddInput(MakeInput(v));
        }
      }
    }
  }

  void CreateConesFromRule(const HornTransStateSpace& space,
                      const HornRule& rule,
                      const ExprSubstitution& var_subst_curr) {
      const auto& activation_var = space.get_activation_var(rule);
      const auto& relation_var = space.get_head_relation_var(rule);
      // The code below fills in cones_, the list of next-state equations for
      // the state variables updated by this rule.
      auto else_elt =
          cones_.insert({relation_var.next(),
                        relation_var.current()}).first;
      else_elt->second = z3::ite(activation_var, rule.ctx().bool_val(true),
                                 else_elt->second);
      for (auto&& zipped : space.iter_args_with_places(rule.head())) {
        z3::expr arg = boost::get<0>(zipped);
        const StateVar& place_var = boost::get<1>(zipped);
        else_elt =
            cones_.insert({place_var.next(),
                          place_var.current()}).first;
        else_elt->second = z3::ite(activation_var, var_subst_curr(arg),
                                   else_elt->second);
      }
      logger.Log(3, "cones fir this rule");
      for (auto&& [next_var, val] : cones_) {
        logger.Log(3, "    {}: {}", next_var, val);
      }
  }

  // outputs to rule_input_vars_
  void CreateQuantifiedVarInputs(const HornTransStateSpace& space,
                                 const vector<HornRule>& rules) {
    SortMap<std::vector<PrimaryInputRef>> rule_places_by_sort;
    for (auto& rule : rules) {
      // Makes any inputs required to express the quantified variables in the
      // rule, binning them by sort
      if (z3::expr rule_expr = rule; rule_expr.is_quantifier()) {
        const unsigned num_bound_vars =
            Z3_get_quantifier_num_bound(rule_expr.ctx(), rule_expr);
        if (config.allocate_inputs_by_sort) {
          SortMap<int> counts;
          for (unsigned i = 0; i < num_bound_vars; i++) {
            z3::sort sort(
                rule_expr.ctx(),
                Z3_get_quantifier_bound_sort(rule_expr.ctx(), rule_expr, i));
            if (counts.find(sort) == counts.end()) {
              counts[sort] = 0;
            }
            counts[sort]++;
          }
          for (auto&& [sort, num_occurrences] : counts) {
            int num_places =
                static_cast<int>(rule_places_by_sort[sort].size());
            for (int i = 0; i < (num_occurrences - num_places); i++) {
              auto name =
                  (SortName(sort) + "-input-" + to_string(num_places+i));
              auto& input_var = space.xsys().AddInput(MakeInput(
                      ctx().constant(name.c_str(), sort)));
              rule_places_by_sort[sort].push_back(input_var);
            }
          }
          counts.clear();
          for (unsigned i = 0; i < num_bound_vars; i++) {
            z3::sort sort(
                rule_expr.ctx(),
                Z3_get_quantifier_bound_sort(rule_expr.ctx(), rule_expr, i));
            auto var = rule_places_by_sort.at(sort)[counts[sort]];
            rule_input_vars_[rule.index()].push_back(var);
            counts[sort]++;
          }
          // Apparently the de-bruijn indices of variables are the revers from
          // the way they would be in a list...there's some verbiage to this
          // effect in the constructor for forall's in the Z3 documentation but
          // I didn't realize it would extend to this API. I guess it does.
          std::reverse(rule_input_vars_[rule.index()].begin(),
                       rule_input_vars_[rule.index()].end());
        } else {
          // Create inputs named input-<rule-number>-<deBruijn index>
          for (unsigned i = 0; i < num_bound_vars; i++) {
            z3::sort sort(
                rule_expr.ctx(),
                Z3_get_quantifier_bound_sort(rule_expr.ctx(), rule_expr, i));
            z3::symbol var_name(
                rule_expr.ctx(),
                Z3_get_quantifier_bound_name(rule_expr.ctx(), rule_expr, i));
            auto name = fmt::format("input-{}-{}", rule.index(), var_name.str());
            auto& input_var = space.xsys().AddInput(
                MakeInput(ctx().constant(name.c_str(), sort)));
            rule_input_vars_[rule.index()].push_back(input_var);
          }
          std::reverse(rule_input_vars_[rule.index()].begin(),
                       rule_input_vars_[rule.index()].end());
        }
      }

    }
  }

  z3::expr trans() const { return trans_; }
  z3::context& ctx() const { return trans_.ctx(); }

  auto queries() {
    return boost::make_iterator_range(queries_.begin(), queries_.end());
  }

  z3::expr translate(const z3::expr& e) {
    return to_vmt_(e);
  }

  const PrimaryInput& get_rule_input_var(const HornRule& r,
                                         const z3::expr& bound_var) const {
    // The inputs are indexed by rule and de-Bruijn index of the bound var
    return rule_input_vars_.at(r.index()).at(
        Z3_get_index_value(bound_var.ctx(), bound_var));
  }

 private:
  // Transition relation that has been translated from the Horn set
  z3::expr trans_;
  // Maps rule index to quantifier var place inputs
  unordered_map<int, vector<PrimaryInputRef>> rule_input_vars_;
  ExprMap<z3::expr> cones_;
  std::vector<HornRule> queries_;
  Horn2VmtRewriter to_vmt_;
  ExprSubstitution make_var_subst(
      const HornTransStateSpace&, const HornRule& rule);
};

ExprSubstitution HornSetTrans::make_var_subst(
    const HornTransStateSpace& space, const HornRule& rule) {
  ExprSubstitution var_subst_curr(rule.ctx());
  // Set of vars mapped to places by visit_map_var()
  IterExprSet vars_with_places;
  // Set of all visited nodes for df traversal
  IterExprSet visited;
  // Precondition: e is a sub-expression of rule.body().
  // visit_map_var(), for a relation occurrence R(a,b,c,...), looks in a,b,c
  // for variable occurrences (as opposed to expressions) and maps them
  // directly to place vars, because this helps EUForia's pre-image
  // computation.
  auto visit_map_var = [&](const z3::expr& e) {
    if (e.is_app()) {
      if (auto e_var = space.find_relation_var(e)) {
        logger.Log(3, "--- visiting relation {}", e.decl());
        for (auto&& zipped : space.iter_args_with_places(e)) {
          z3::expr arg = boost::get<0>(zipped);
          const StateVar& place_var = boost::get<1>(zipped);
          if (arg.is_var()) {
            vars_with_places.insert(arg);
            logger.Log(3, "adding place_var [{} / {}] to var_subst_curr",
                       arg, place_var);
            var_subst_curr.AddSubstitution(arg, place_var.current());
          }
        }
      }
    }
  };
  // Precondition: e is a sub-expression of anything in rule that hasn't been
  // visited by visit_map_var().
  auto visit_create_input = [&](const z3::expr& e) {
    if (e.is_var()) {
      // for (unsigned i = 0; i < rule_input_vars.size(); i++) {
      //   auto input = rule_input_vars[i];
      //   logger.Log(3, "    rule_place {}: {}:{}", i, input, input.get_sort());
      // }
      // const z3::expr& i_expr = rule_input_vars[Z3_get_index_value(e.ctx(), e)];
      const z3::expr& i_expr = get_rule_input_var(rule, e);
      // auto fresh_input =
      //     ctx().constant(("y-" + to_string(counter_++)).c_str(), e.get_sort());
      // auto& input = xsys_.AddInput(MakeInput(fresh_input));
      var_subst_curr.AddSubstitution(e, i_expr);
      logger.Log(3, "adding input [{} / {}] to var_subst_curr", e, i_expr);
    }
  };

  // put var -> place in subst for anything in body
  for_each(df_expr_ext_iterator::begin(rule.body(), visited),
           df_expr_ext_iterator::end(rule.body(), visited), visit_map_var);
  // fill in rest woth var -> input, over body and head, but skip visiting any
  // vars already mapped to places
  visited = vars_with_places;
  for_each(df_expr_ext_iterator::begin(rule.body(), visited),
           df_expr_ext_iterator::end(rule.body(), visited), visit_create_input);
  for_each(df_expr_ext_iterator::begin(rule.head(), visited),
           df_expr_ext_iterator::end(rule.head(), visited), visit_create_input);
  return var_subst_curr;
}

} // end anonymous namespace

//^----------------------------------------------------------------------------^
//

class Horn2Vmt {
 public:
  Horn2Vmt(const HornTransStateSpace& space, const vector<HornRule>& all_rules)
      : state_space_(space), rules_(all_rules) {
    // Sets transition relation
    HornSetTrans all_trans(state_space_, all_rules);
    xsys().set_trans(all_trans.trans());

    // Sets property
    for (const auto& rule : all_trans.queries()) {
      if (is_not(rule.head()) &&
          rule.head().arg(0).decl().decl_kind() == Z3_OP_UNINTERPRETED) {
        // property is denoted by (assert !R)
        xsys().set_property(all_trans.translate(rule.head()));
      } else if (z3::eq(rule.head(), ctx().bool_val(false))) {
        // property is denoted by (R => false)
        xsys().set_property(!all_trans.translate(rule.body()));
      } else {
        EUFORIA_FATAL("unrecognized rule: {} => {}", rule.body(), rule.head());
      }
    }

    // Sets initial state. In the initial state, all relations are false.
    z3::expr init(ctx());
    boost::transform(
        state_space_.relation_vars(), ExprAndInserter(init),
        [&](auto&& var) { return !var.current(); });
    xsys().set_init_state(init);

    // Performs normalizations that make Z3 a happy boi
    NnfRewriter nnf;
    ExprFlattener flatten;
    auto normalize = [&](auto&& e) {
      nnf.Reset(); flatten.Reset(); return flatten(nnf(e)); };
    xsys().RewriteSystem(normalize);
  }

  void Print(std::ostream& os) const {
    int64_t index = 0;
    fmt::print(os, "; {} rules\n", rules_.size());
    fmt::print(os, "; {} relations\n", state_space_.num_relations());
    fmt::print(os, "; {} state vars\n", xsys().num_vars());
    for (auto&& var : xsys().vars()) {
      fmt::print(os, "{}\n", var.current().decl());
      fmt::print(os, "{}\n", var.next().decl());
      fmt::print(os, "(define-fun .def{} () {} (! {} :next {}))\n", index,
                 var.current().get_sort(), var.current(),
                 var.next());
      ++index;
    }
    fmt::print(os, "; {} inputs\n", xsys().num_inputs());
    for (const z3::expr& input : xsys().inputs()) {
      fmt::print(os, "{}\n", input.decl());
    }
    fmt::print(os, "(define-fun .def{} () Bool (! {} :init true))\n",
               index++, xsys().init_state());
    fmt::print(os, "(define-fun .def{} () Bool (! {} :trans true))\n",
               index++, xsys().trans());
    fmt::print(os, "(define-fun .def{} () Bool (! {} :invar-property 0))\n",
               index, xsys().property());
  }

  // XXX instead of vmt print this in SMT2??????
  void PrintTrans(std::ostream& os) {
    int64_t index = 0;
    fmt::print(os, "; {} rules\n", rules_.size());
    fmt::print(os, "; {} relations\n", state_space_.num_relations());
    fmt::print(os, "; {} state vars\n", xsys().num_vars());
    fmt::print(os, "; begin state vars\n");
    // XXX print all declares in a block then print the defines after in a block so they're easier to delet
    for (auto&& var : xsys().vars()) {
      fmt::print(os, "{}\n", var.current().decl());
      fmt::print(os, "{}\n", var.next().decl());
      // fmt::print(os, "(define-fun .def{} () {} (! {} :next {}))\n", index,
      //            var.current().get_sort(), var.current(),
      //            var.next());
      ++index;
    }
    fmt::print(os, "; end state vars\n");
    fmt::print(os, "; {} inputs\n", xsys().num_inputs());
    fmt::print(os, "; begin inputs\n");
    for (const z3::expr& input : xsys().inputs()) {
      fmt::print(os, "{}\n", input.decl());
    }
    fmt::print(os, "; end inputs\n");
    fmt::print(os, "(declare-fun .trans () Bool)\n");
    fmt::print(os, "(assert (= .trans {}))\n", xsys().trans());
    // fmt::print(os, "(define-fun .def{} () Bool (! {} :trans true))\n",
    //            index++, xsys().trans());
  }

  z3::context& ctx() const { return state_space_.ctx(); }

 private:
  const HornTransStateSpace& state_space_;
  const vector<HornRule>& rules_;

  const VmtTransitionSystem& xsys() const { return state_space_.xsys(); }
  VmtTransitionSystem& xsys() { return state_space_.xsys(); }
};

//^----------------------------------------------------------------------------^
//
namespace po = boost::program_options;

po::options_description desc("options");

int main(int argc, char **argv) {
  // option parsing
  desc.add_options()
      ("help,h", "help")
      ("v", po::value<int>()->default_value(0), "set log level")
      ("int2bv", "convert ints to bvs as well")
      ("slice", po::value<string>()->default_value(""),
       "output sliced transition using given prefix (suppresses normal output)")
      ("onehots", po::value<bool>()->default_value(true),
       "enable explicit one-hot transition constraints")
      ("varmap", po::value<string>(),
       "output mapping between relation places and state variables to this file")
      ("strategy", po::value<string>()->default_value("relation"),
       "sort - allocates one state variable per distinct place of a given sort\nrelation - allocates one state variable per relation place")
      ("input-file", po::value<string>(), "SMT2 file to convert");
  po::positional_options_description pd;
  pd.add("input-file", -1);
  po::variables_map opts;
  po::store(po::command_line_parser(argc, argv).
            options(desc).positional(pd).run(), opts);
  po::notify(opts);

  if (!opts.count("input-file")) {
    fmt::print(cerr, "error: missing input-file\n");
  }
  if (opts.count("help") || !opts.count("input-file")) {
    fmt::print(cout, "{} input-file\n", argv[0]);
    fmt::print(cout, "{}\n", desc);
    return EXIT_FAILURE;
  }

  logger.set_level(opts["v"].as<int>());

  config.enable_one_hots = opts["onehots"].as<bool>();

  config.allocate_vars_by_sort = true;
  if (opts["strategy"].as<string>() == "relation") {
    config.allocate_vars_by_sort = false;
  }
  if (auto strat = opts["strategy"].as<string>();
      !(strat == "sort" || strat == "relation")) {
    fmt::print(cerr, "error: unknown variable allocation strategy: {}\n",
               strat);
    return EXIT_FAILURE;
  }

  if (opts.count("varmap")) {
    config.varmap_filename = string(opts["varmap"].as<string>());
  }

  auto slice_prefix = opts["slice"].as<string>();
  config.slice_t = slice_prefix != "";

  z3::context ctx;
  config.input_filename = opts["input-file"].as<string>();
  HornClauses clause_db(ctx, config.input_filename.c_str(),
                        bool(opts.count("int2bv")));
  if (config.slice_t) {
    logger.Log(1, "config: slicing T into multiple files");
    int i = 0;
    auto head2trans = clause_db.group_by_heads();
    vector<HornTransition> queries;
    VmtTransitionSystem xsys(ctx);
    vector<HornRule> all_rules;
    boost::copy(clause_db.rules(), back_inserter(all_rules));
    HornTransStateSpace space(config, all_rules, &xsys);
    for (auto&& [head, horn_trans] : head2trans) {
      if (boost::algorithm::all_of(
              horn_trans.rules(),
              [](const HornRule& r) { return r.is_query(); })) {
        logger.Log(0, "skipping {} queries due to T slicing",
                   horn_trans.rules().size());
        continue;
      }

      // Inputs for quantified vars will be created during translation to Vmt;
      // make sure those go into xsys_copy since they shouldn't interact with
      // other slices of T.
      VmtTransitionSystem xsys_copy(xsys);
      space.set_output_xsys(&xsys_copy);
      logger.Log(1, "translating {} clauses for slice of {}",
                 horn_trans.rules().size(), head.name());
      std::vector<HornRule> rules;
      boost::copy(horn_trans.rules(), back_inserter(rules));
      Horn2Vmt h2v(space, rules);
      ofstream trans_out(fmt::format("{}{}.vmt", slice_prefix, i));
      fmt::print(trans_out, "; common head: {}\n", head.name());
      h2v.PrintTrans(trans_out);
      logger.Log(1, "slice of {} written to {}{}.vmt", head.name(),
                 slice_prefix, i);
      i++;
    }
  } else {
    VmtTransitionSystem xsys(ctx);
    std::vector<HornRule> all_rules;
    boost::copy(clause_db.rules(), back_inserter(all_rules));
    HornTransStateSpace space(config, all_rules, &xsys);
    Horn2Vmt h2v(space, all_rules);
    h2v.Print(std::cout);
  }

  if (config.varmap_filename) {
    logger.Log(1, "wrote varmap to {}", *config.varmap_filename);
  }

  return EXIT_SUCCESS;
}


// ===============================================================================================================
// Copyright (c) 2019, Cornell University. All rights reserved.
//
// 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 otice, 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 Cornell University 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 HOLDERS 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.
//
// Author: Kai Zhang (kz298@cornell.edu)
//
// The research is based upon work supported by the Office of the Director of National Intelligence (ODNI),
// Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287.
// The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes.
// ===============================================================================================================
//
//
// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// 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 ETH Zurich and UNC Chapel Hill 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 HOLDERS 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.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#include "mvs/image.h"

#include <Eigen/Core>

#include "base/projection.h"
#include "util/logging.h"

namespace colmap {
namespace mvs {

// helper function
inline void Compute4by4ProjectionMatrix(const double K[9],
		const double R[9],
		const double T[9],
		const double last_row[4],
		double P[16], double inv_P[16]) {
	  // 3 by 4 projection matrix
	  Eigen::Matrix<double, 3, 4, Eigen::RowMajor> P_3by4;
	  P_3by4.leftCols<3>() = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(R);
	  P_3by4.rightCols<1>() = Eigen::Map<const Eigen::Matrix<double, 3, 1>>(T);
	  P_3by4 = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(K) * P_3by4;

	  // 4 by 4 projection matrix
	  Eigen::Matrix<double, 4, 4, Eigen::RowMajor> P_4by4;
	  P_4by4.block<3, 4>(0, 0) = P_3by4;
	  P_4by4.row(3) = Eigen::Map<const Eigen::Matrix<double, 1, 4>>(last_row);

	  // try to make the projection matrix more numerically stable
	  // scale all the numbers to lie in [0, 10]
	  P_4by4 *= (10.0 / P_4by4.maxCoeff());

	  memcpy(P, P_4by4.data(), 16 * sizeof(double));

	  Eigen::Matrix<double, 4, 4, Eigen::RowMajor> inv_P_4by4 = P_4by4.inverse();

	  inv_P_4by4 *= (10.0 / inv_P_4by4.maxCoeff());

	  memcpy(inv_P, inv_P_4by4.data(), 16 * sizeof(double));
}

// helper function
inline void ComputeProjectionCenter(const double R[9], const double T[9],
									double C[3]) {
	  // 3 by 4 projection matrix
	  Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R_mat(R);
	  Eigen::Map<const Eigen::Matrix<double, 3, 1>> T_mat(T);
	  const Eigen::Matrix<double, 3, 1> C_mat = -R_mat.transpose() * T_mat;

	  memcpy(C, C_mat.data(), 3 * sizeof(double));
}

// helper function
inline void DoubleArrToFloatArr(const double* double_arr, float* float_arr, int cnt) {
	for (int i=0; i<cnt; ++i) {
		float_arr[i] = (float) double_arr[i];
	}
}

Image::Image(const std::string& path, const size_t width, const size_t height,
             const double K[9], const double R[9], const double T[3])
    : path_(path), width_(width), height_(height) {
  memcpy(K_, K, 9 * sizeof(double));
  memcpy(R_, R, 9 * sizeof(double));
  memcpy(T_, T, 3 * sizeof(double));
}

void Image::SetBitmap(const Bitmap& bitmap) {
  bitmap_ = bitmap;
  CHECK_EQ(width_, bitmap_.Width());
  CHECK_EQ(height_, bitmap_.Height());
}

void Image::SetK(const double K[9]) const {
	memcpy(K_, K, 9 * sizeof(double));
}

size_t Image::GetWidth() const { return width_; }

size_t Image::GetHeight() const { return height_; }

const Bitmap& Image::GetBitmap() const { return bitmap_; }

const std::string& Image::GetPath() const { return path_; }

void Image::SetLastRow(const double last_row[4]) {
  memcpy(last_row_, last_row, 4 * sizeof(double));
}

float Image::GetDepth(double x, double y, double z) const {
	  // 3 by 4 projection matrix
	  Eigen::Matrix<double, 3, 4, Eigen::RowMajor> P_3by4;
	  P_3by4.leftCols<3>() = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(R_);
	  P_3by4.rightCols<1>() = Eigen::Map<const Eigen::Matrix<double, 3, 1>>(T_);
	  P_3by4 = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(K_) * P_3by4;

	  // 4 by 4 projection matrix
	  Eigen::Matrix<double, 4, 4, Eigen::RowMajor> P_4by4;
	  P_4by4.block<3, 4>(0, 0) = P_3by4;
	  P_4by4.row(3) = Eigen::Map<const Eigen::Matrix<double, 1, 4>>(last_row_);

	  const Eigen::Matrix<double, 4, 1> X(x, y, z, 1.0);

	  Eigen::Matrix<double, 4, 1> result = P_4by4 * X;
	  // depth is the fourth component
	  double depth = result[3] / result[2];

	  // low precision output
	  return (float) depth;
}

const double* Image::GetLastRow() const {
	return last_row_;
}

void Image::GetK(float K[9]) const {
	DoubleArrToFloatArr(K_, K, 9);
}

void Image::GetRT(float R[9],  float T[3]) const {
	DoubleArrToFloatArr(R_, R, 9);
	DoubleArrToFloatArr(T_, T, 3);
}

void Image::GetC(float C[3]) const {
	double C_double[3];
	ComputeProjectionCenter(R_, T_, C_double);
	DoubleArrToFloatArr(C_double, C, 3);
}

void Image::GetCDouble(double C[3]) const {
	ComputeProjectionCenter(R_, T_, C);
}

void Image::GetKDouble(double K[9]) const {
	memcpy(K, K_, 9*sizeof(double));
}

void Image::GetPinvP(float P[16], float inv_P[16]) const {
	// compute projection matrix
	double P_double[16];
	double inv_P_double[16];
	Compute4by4ProjectionMatrix(K_, R_, T_, last_row_, P_double, inv_P_double);

	// convert to low-precision
	DoubleArrToFloatArr(P_double, P, 16);
	DoubleArrToFloatArr(inv_P_double, inv_P, 16);
}

void Image::GetPinvPDouble(double P[16], double inv_P[16]) const {
	// compute projection matrix
	Compute4by4ProjectionMatrix(K_, R_, T_, last_row_, P, inv_P);
}

// low-precision output
void Image::Original(float K[9], float inv_K[9], float R[9], float T[3], float P[16], float inv_P[16], float C[3]) const{
	// compute projection matrix
	double P_double[16];
	double inv_P_double[16];
	Compute4by4ProjectionMatrix(K_, R_, T_, last_row_, P_double, inv_P_double);

	double C_double[3];
	ComputeProjectionCenter(R_, T_, C_double);

	// compute inverse K
	const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K_mat(K_);
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_mat_inv = K_mat.inverse();
	const double *K_mat_inv_data = K_mat_inv.data();

	// convert to low-precision
	DoubleArrToFloatArr(K_, K, 9);
	DoubleArrToFloatArr(K_mat_inv_data, inv_K, 9);
	DoubleArrToFloatArr(R_, R, 9);
	DoubleArrToFloatArr(T_, T, 3);
	DoubleArrToFloatArr(P_double, P, 16);
	DoubleArrToFloatArr(inv_P_double, inv_P, 16);
	DoubleArrToFloatArr(C_double, C, 3);
}

inline void MatrixPrint(float *mat, int row_cnt, int col_cnt) {
	for (int i=0; i<row_cnt; ++i) {
		for (int j=0; j<col_cnt; ++j) {
			std::cout << mat[i*col_cnt+j] << ", ";
		}
	}
}

//void Image::Rotate90Multi_test(int cnt) const {
//	float K[9];
//	float R[9];
//	float T[3];
//	float P[16];
//	float inv_P[16];
//	float C[3];
//
//	Rotate90Multi(cnt, K, R, T, P, inv_P, C);
//
//	float K_float[9];
//	DoubleArrToFloatArr(K_, K_float, 9);
//	std::cout << "\nrot=0, K_: ";
//	MatrixPrint(K_float, 3, 3);
//	std::cout << "\nwidth, height: " << width_ << ", " << height_;
//	std::cout << "\nrot=" << cnt << ", K: ";
//	MatrixPrint(K, 3, 3);
//
//	float R_float[9];
//	DoubleArrToFloatArr(R_, R_float, 9);
//	std::cout << "\nrot=0, R_: ";
//	MatrixPrint(R_float, 3, 3);
//	std::cout << "\nrot=" << cnt << ", R: ";
//    MatrixPrint(R, 3, 3);
//
//	float T_float[3];
//	DoubleArrToFloatArr(T_, T_float, 3);
//	std::cout << "\nrot=0, T_: ";
//	MatrixPrint(T_float, 3, 1);
//	std::cout << "\nrot=" << cnt << ", T: ";
//	MatrixPrint(T, 3, 1);
//
//	float last_row_float[4];
//	DoubleArrToFloatArr(last_row_, last_row_float, 4);
//	std::cout << "\nlast_row_: ";
//	MatrixPrint(last_row_float, 1, 4);
//	std::cout << "\nrot=" << cnt << ", P: ";
//	MatrixPrint(P, 4, 4);
//	std::cout << "\nrot=" << cnt << ", inv_P: ";
//	MatrixPrint(inv_P, 4, 4);
//
//	std::cout << std::endl;
//}

void Image::Rotate90Multi(int cnt, float K[9], float inv_K[9], float R[9], float T[3], float P[16], float inv_P[16], float C[3]) const {
	switch (cnt % 4) {
	case 0:
		Original(K, inv_K, R, T, P, inv_P, C);
		break;
	case 1:
		Rotate90(K, inv_K, R, T, P, inv_P, C);
		break;
	case 2:
		Rotate180(K, inv_K, R, T, P, inv_P, C);
		break;
	case 3:
		Rotate270(K, inv_K, R, T, P, inv_P, C);
		break;
	default:
		break;
	}
}

// rotate the camera coordinate frame by 90 degrees
// a 3d point (X, Y, Z) in camera coordinate frame is rotated to (Y, -X, Z)
// the corresponing pixel (u, v) is rotated to (v, w-1-u)
// [u; v; 1] = K[X; Y; Z]
// needs to modify the intrinsics so that the new mapping holds, i.e.,
// [v; w-1-u; 1] = K'[Y; -X; Z]
// first, [X; Y; Z]= [0, -1, 0; 1, 0, 0; 0, 0, 1][Y; -X; Z]
// then, [v; w-1-u; 1] = [0, 1, 0; -1, 0, w-1; 0, 0, 1][u; v; 1]
void Image::Rotate90(float K[9], float inv_K[9], float R[9], float T[3], float P[16], float inv_P[16], float C[3]) const {
    // modify intrinsics
    const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K_old(K_);
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> A;
    A << 0, 1, 0, -1, 0, width_-1, 0, 0, 1;
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> B;
    B << 0, -1, 0, 1, 0, 0, 0, 0, 1;

    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_new = A * K_old * B;
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_new_inv = K_new.inverse();

	// modify extrinsics
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rot;
	rot << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
	const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R_old(R_);
	const Eigen::Map<const Eigen::Matrix<double, 3, 1>> T_old(T_);

	const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R_new = rot * R_old;
	const Eigen::Matrix<double, 3, 1> T_new = rot * T_old;

	const double *K_new_data = K_new.data();
	const double *K_new_inv_data = K_new_inv.data();
	const double *R_new_data = R_new.data();
	const double *T_new_data = T_new.data();

	// compute projection matrix
	double P_new_double[16];
	double inv_P_new_double[16];
	Compute4by4ProjectionMatrix(K_new_data, R_new_data, T_new_data, last_row_, P_new_double, inv_P_new_double);

	// compute projection center
	double C_new_double[3];
	ComputeProjectionCenter(R_new_data, T_new_data, C_new_double);

	// convert to low-precision
	DoubleArrToFloatArr(K_new_data, K, 9);
	DoubleArrToFloatArr(K_new_inv_data, inv_K, 9);
	DoubleArrToFloatArr(R_new_data, R, 9);
	DoubleArrToFloatArr(T_new_data, T, 3);
	DoubleArrToFloatArr(P_new_double, P, 16);
	DoubleArrToFloatArr(inv_P_new_double, inv_P, 16);
	DoubleArrToFloatArr(C_new_double, C, 3);
}

// a 3d point (X, Y, Z) in camera coordinate frame is rotated to (-X, -Y, Z)
// the corresponing pixel (u, v) is rotated to (w-1-u, h-1-v)
// [u; v; 1] = K[X; Y; Z]
// needs to modify the intrinsics so that the new mapping holds, i.e.,
// [w-1-u; h-1-v ; 1] = K'[-X; -Y; Z]
// first, [X; Y; Z]= [-1, 0, 0; 0, -1, 0; 0, 0, 1][-X; -Y; Z]
// then, [w-1-u; h-1-v; 1] = [-1, 0, w-1; 0, -1, h-1; 0, 0, 1][u; v; 1]
void Image::Rotate180(float K[9], float inv_K[9], float R[9], float T[3], float P[16], float inv_P[16], float C[3]) const {
    // modify intrinsics
    const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K_old(K_);
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> A;
    A << -1, 0, width_-1, 0, -1, height_-1, 0, 0, 1;
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> B;
    B << -1, 0, 0, 0, -1, 0, 0, 0, 1;

    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_new = A * K_old * B;
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_new_inv = K_new.inverse();

	// modify extrinsics
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rot;
	rot << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
	rot = rot * rot;
	const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R_old(R_);
	const Eigen::Map<const Eigen::Matrix<double, 3, 1>> T_old(T_);

	const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R_new = rot * R_old;
	const Eigen::Matrix<double, 3, 1> T_new = rot * T_old;

	const double *K_new_data = K_new.data();
	const double *K_new_inv_data = K_new_inv.data();
	const double *R_new_data = R_new.data();
	const double *T_new_data = T_new.data();

	// compute projection matrix
	double P_new_double[16];
	double inv_P_new_double[16];
	Compute4by4ProjectionMatrix(K_new_data, R_new_data, T_new_data, last_row_, P_new_double, inv_P_new_double);

	// compute projection center
	double C_new_double[3];
	ComputeProjectionCenter(R_new_data, T_new_data, C_new_double);

	// convert to low-precision
	DoubleArrToFloatArr(K_new_data, K, 9);
	DoubleArrToFloatArr(K_new_inv_data, inv_K, 9);
	DoubleArrToFloatArr(R_new_data, R, 9);
	DoubleArrToFloatArr(T_new_data, T, 3);
	DoubleArrToFloatArr(P_new_double, P, 16);
	DoubleArrToFloatArr(inv_P_new_double, inv_P, 16);
	DoubleArrToFloatArr(C_new_double, C, 3);
}

// a 3d point (X, Y, Z) in camera coordinate frame is rotated to (-Y, X, Z)
// the corresponing pixel (u, v) is rotated to (h-1-v, u)
// needs to modify the intrinsics so that the new mapping holds, i.e.,
// [h-1-v; u ; 1] = K'[-Y; X; Z]
// first, [X; Y; Z]= [0, 1, 0; -1, 0, 0; 0, 0, 1][-Y; X; Z]
// then, [h-1-v; u; 1] = [0, -1, h-1; 1, 0, 0; 0, 0, 1][u; v; 1]
void Image::Rotate270(float K[9], float inv_K[9], float R[9], float T[3], float P[16], float inv_P[16], float C[3]) const {
    // modify intrinsics
    const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> K_old(K_);
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> A;
    A << 0, -1, height_-1, 1, 0, 0, 0, 0, 1;
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> B;
    B << 0, 1, 0, -1, 0, 0, 0, 0, 1;

    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_new = A * K_old * B;
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_new_inv = K_new.inverse();

    // modify extrinsics
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rot;
	rot << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0;
	rot = rot * rot * rot;
	const Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R_old(R_);
	const Eigen::Map<const Eigen::Matrix<double, 3, 1>> T_old(T_);

	const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R_new = rot * R_old;
	const Eigen::Matrix<double, 3, 1> T_new = rot * T_old;

	const double *K_new_data = K_new.data();
	const double *K_new_inv_data = K_new_inv.data();
	const double *R_new_data = R_new.data();
	const double *T_new_data = T_new.data();

	// compute projection matrix
	double P_new_double[16];
	double inv_P_new_double[16];
	Compute4by4ProjectionMatrix(K_new_data, R_new_data, T_new_data, last_row_, P_new_double, inv_P_new_double);

	// compute projection center
	double C_new_double[3];
	ComputeProjectionCenter(R_new_data, T_new_data, C_new_double);

	// convert to low-precision
	DoubleArrToFloatArr(K_new_data, K, 9);
	DoubleArrToFloatArr(K_new_inv_data, inv_K, 9);
	DoubleArrToFloatArr(R_new_data, R, 9);
	DoubleArrToFloatArr(T_new_data, T, 3);
	DoubleArrToFloatArr(P_new_double, P, 16);
	DoubleArrToFloatArr(inv_P_new_double, inv_P, 16);
	DoubleArrToFloatArr(C_new_double, C, 3);
}

void Image::Rescale(const float factor) { Rescale(factor, factor); }

void Image::Rescale(const float factor_x, const float factor_y) {
  const size_t new_width = std::round(width_ * factor_x);
  const size_t new_height = std::round(height_ * factor_y);

  if (bitmap_.Data() != nullptr) {
    bitmap_.Rescale(new_width, new_height);
  }

  const double scale_x = new_width / static_cast<float>(width_);
  const double scale_y = new_height / static_cast<float>(height_);
  K_[0] *= scale_x;
  K_[1] *= scale_x;
  K_[2] *= scale_x;
  K_[4] *= scale_y;
  K_[5] *= scale_y;

  width_ = new_width;
  height_ = new_height;
}

void Image::Downsize(const size_t max_width, const size_t max_height) {
  if (width_ <= max_width && height_ <= max_height) {
    return;
  }
  const float factor_x = static_cast<float>(max_width) / width_;
  const float factor_y = static_cast<float>(max_height) / height_;
  Rescale(std::min(factor_x, factor_y));
}

}  // namespace mvs
}  // namespace colmap


/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy (DOE). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

/*!
 * \file calibrate_share_weight_visitor.cpp
 * \ingroup Objects
 * \brief The CalibrateShareWeightVisitor class source file.
 * \author Pralit Patel
 */

#include "util/base/include/definitions.h"
#include <cassert>
#include <boost/math/tr1.hpp>

#include "util/base/include/calibrate_share_weight_visitor.h"
#include "technologies/include/technology_container.h"
#include "technologies/include/itechnology.h"
#include "sectors/include/subsector.h"
#include "sectors/include/nesting_subsector.h"
#include "sectors/include/sector.h"
#include "containers/include/gdp.h"
#include "util/logger/include/ilogger.h"
#include "functions/include/idiscrete_choice.hpp"

using namespace std;

extern Scenario* scenario;

/*!
 * \brief Constructor
 * \param aRegionName Name of the region if starting the visiting below the
 *        region level.
 */
CalibrateShareWeightVisitor::CalibrateShareWeightVisitor( const string& aRegionName, const GDP* aGDP )
:mCurrentRegionName( aRegionName ), mGDP( aGDP )
{
}

/*!
 * \brief A generic method to calculate share-weights based off of calibrated outputs
 * \details This method will make share-weights relative to the child container that has
 *          has the largest share, which will be given a share-weight of 1.
 *
 *          Note that because we want to generalize this routine to avoid code duplication
 *          but we have to deal with ContainerType that do not share a common interface we
 *          will rely on the following adaptor methods being defined to supply the behavior
 *          appropriate for the derived types that this tempalted function gets instatiated
 *          with:
 *            - getChildren: to get a vector of child containers to calculate share-weights for
 *            - isAvailable: determine if this child is "available" (i.e. not fixed)
 *            - getCalValue: get calibrated output of child to determine share
 *            - getCost: get cost of child
 *            - getFuelPrefElasticity: get fuel preference elasticity of child
 *            - getDiscreteChoice: get the discrete choice function to use
 *            - setShareWeight: sets the calibrated share-weight into the child
 *
 * \param aContainer The container who's children need to have their share-weights calibrated.
 * \param aPeriod The current model period.
 */
template<typename ContainerType>
void CalibrateShareWeightVisitor::calibrateShareWeights( const ContainerType* aContainer, const int aPeriod ) const
{
    // Find out if we need to do calibration, make sure we do not have a mix of calibrated/non-calibrated
    // children, and figure out the largest child to make the other children anchored by it.
    int anchorIndex = -1;
    bool hasCalValues = false;
    double maxCalValue = 0;
    int numCalChildren = 0;
    double totalCalValue = 0;
    auto childrenVec = getChildren( aContainer, aPeriod );
    for( int childIndex = 0; childIndex < childrenVec.size(); ++childIndex ) {
        double currCalValue = getCalValue( childrenVec[ childIndex ], aPeriod );
        bool isAvail = isAvailable( childrenVec[ childIndex ], aPeriod );

        // check if the child is calibrated
        if( hasCalValues && currCalValue <= 0 && isAvail ) {
            // warn that we mixed calibrated and variable children
            ILogger& mainLog = ILogger::getLogger( "main_log" );
            ILogger& calibrationLog = ILogger::getLogger( "calibration_log" );
            mainLog.setLevel( ILogger::WARNING );
            calibrationLog.setLevel( ILogger::WARNING );
            mainLog << "Mixed calibrated and variable children or read a zero calibration value in Region: "
                    << mCurrentRegionName << " in sector: " << mCurrentSectorName
                    << " for container: " << childrenVec[ childIndex ]->getName() << endl;
            calibrationLog << "Mixed calibrated and variable children or read a zero calibration value in Region: "
                    << mCurrentRegionName << " in sector: " << mCurrentSectorName
                    << " for container: " << childrenVec[ childIndex ]->getName() << endl;
        }
        else if( currCalValue > 0 ) {
            hasCalValues = true;
        }

        // If the calibrated value > 0 then increase the number of children to calibrate
        // and include it in the total sum.
        if( currCalValue > 0 ) {
            ++numCalChildren;
            totalCalValue += currCalValue;
        }

        // attempt to locate the largest child
        if( currCalValue > maxCalValue ) {
            maxCalValue = currCalValue;
            anchorIndex = childIndex;
        }
    }

    // If we are using the abosolute choice function, then we need to
    // set the base cost for the model.  We will calculate an appropriate
    // base cost and let the discrete choice function use it if it needs it.

    // The base cost is equal to the highest cost child that has a share if we
    // have calibration data.  Otherwise, it is equal to the highest cost child
    // that has a valid cost; otherwise, we may be in trouble if a user did not
    // parse a value.
    double baseCost = 0;
    for( int childIndex = 0; childIndex < childrenVec.size(); ++childIndex ) {
        double currCost = getCost( childrenVec[ childIndex ], aPeriod );
        if( !boost::math::isnan( currCost )  && currCost > baseCost &&
           ( ( hasCalValues && getCalValue( childrenVec[ childIndex ], aPeriod ) > 0 ) || !hasCalValues ) )
        {
            baseCost = currCost;
        }
    }
    // In the case where there is only one child we will reset the base-price
    // to one as the sharing should not matter.
    if( baseCost == 0 && childrenVec.size() == 1 ) {
        baseCost = 1;
    }
    // baseCost may still be zero and if a user did not parse a base cost an error
    // will be generated.
    IDiscreteChoice* choiceFn = getDiscreteChoice( aContainer );
    choiceFn->setBaseValue( baseCost );

    // do calibration if we have cal values and there are more than one child in this nest
    if( hasCalValues && numCalChildren > 1 ) {
        // we should have found a child to have share weights anchored
        assert( anchorIndex != -1 );
        const double scaledGdpPerCapita = mGDP->getBestScaledGDPperCap( aPeriod );
        const double anchorCost = getCost( childrenVec[ anchorIndex ], aPeriod );
        const double anchorShare = ( getCalValue( childrenVec[ anchorIndex ], aPeriod )
                                    / totalCalValue ) / pow( scaledGdpPerCapita, getFuelPrefElasticity( childrenVec[ anchorIndex ], aPeriod ) );
        assert( anchorShare > 0 );

        for( int childIndex = 0; childIndex < childrenVec.size(); ++childIndex ) {
            auto currChild = childrenVec[ childIndex ];
            double currShare = ( getCalValue( currChild, aPeriod ) / totalCalValue )
                / pow( scaledGdpPerCapita, getFuelPrefElasticity( currChild, aPeriod ) );

            // only set the share weight for valid children
            if( currShare > 0 ) {
                double currCost = getCost( currChild, aPeriod );

                double currShareWeight = choiceFn->calcShareWeight( currShare, currCost,
                                                                    anchorShare, anchorCost, aPeriod );
                setShareWeight( currChild, currShareWeight, aPeriod );
            }
        }
    }

}

// adaptor methods that allows us to generically define calibrateShareWeights even
// though Sector/Subsector/Technology do not share a common interface
std::vector<Subsector*> CalibrateShareWeightVisitor::getChildren( const Sector* aSector, const int aPeriod ) const {
    return aSector->mSubsectors;
}
std::vector<Subsector*> CalibrateShareWeightVisitor::getChildren( const NestingSubsector* aSubsector, const int aPeriod ) const {
    return aSubsector->mSubsectors;
}
std::vector<ITechnology*> CalibrateShareWeightVisitor::getChildren( const Subsector* aSubsector, const int aPeriod ) const {
    vector<ITechnology*> ret( aSubsector->mTechContainers.size() );
    size_t index = 0;
    for( auto techContainer : aSubsector->mTechContainers ) {
        ret[index++] = techContainer->getNewVintageTechnology( aPeriod );
    }
    return ret;
}

bool CalibrateShareWeightVisitor::isAvailable( const Subsector* aSubsector, const int aPeriod ) const {
    return !( aSubsector->containsOnlyFixedOutputTechnologies( aPeriod )
        || aSubsector->mShareWeights[ aPeriod ] == 0 );
}
bool CalibrateShareWeightVisitor::isAvailable( const ITechnology* aTechnology, const int aPeriod ) const {
    return aTechnology->isAvailable( aPeriod );
}

double CalibrateShareWeightVisitor::getCalValue( const Subsector* aSubsector, const int aPeriod ) const {
    return aSubsector->getTotalCalOutputs( aPeriod );
}
double CalibrateShareWeightVisitor::getCalValue( const ITechnology* aTechnology, const int aPeriod ) const {
    const bool hasRequired = false;
    const string requiredName = "";
    return aTechnology->getCalibrationOutput( hasRequired, requiredName, aPeriod );
}

double CalibrateShareWeightVisitor::getCost( const Subsector* aSubsector, const int aPeriod ) const {
    return aSubsector->getPrice( mGDP, aPeriod );
}
double CalibrateShareWeightVisitor::getCost( const ITechnology* aTechnology, const int aPeriod ) const {
    return aTechnology->getCost( aPeriod );
}

double CalibrateShareWeightVisitor::getFuelPrefElasticity( const Subsector* aSubsector, const int aPeriod ) const {
    return aSubsector->mFuelPrefElasticity[ aPeriod ];
}
double CalibrateShareWeightVisitor::getFuelPrefElasticity( const ITechnology* aTechnology, const int aPeriod ) const {
    return aTechnology->calcFuelPrefElasticity( aPeriod );
}

IDiscreteChoice* CalibrateShareWeightVisitor::getDiscreteChoice( const Sector* aSector ) const {
    return aSector->mDiscreteChoiceModel;
}
IDiscreteChoice* CalibrateShareWeightVisitor::getDiscreteChoice( const Subsector* aSubsector ) const {
    return aSubsector->mDiscreteChoiceModel;
}

void CalibrateShareWeightVisitor::setShareWeight( Subsector* aSubsector,
                                                  const double aShareWeight,
                                                  const int aPeriod ) const
{
    aSubsector->mShareWeights[ aPeriod ] = aShareWeight;
}
void CalibrateShareWeightVisitor::setShareWeight( ITechnology* aTechnology,
                                                  const double aShareWeight,
                                                  const int aPeriod ) const
{
    aTechnology->setShareWeight( aShareWeight );
}

void CalibrateShareWeightVisitor::startVisitSector( const Sector* aSector, const int aPeriod ) {
    mCurrentSectorName = aSector->getName();
}

void CalibrateShareWeightVisitor::endVisitSector( const Sector* aSector, const int aPeriod ) {
    // Doing subsector calibration in sector rather than in subsector because we will need access
    // to all of the subsectors at the same time.  Also we need to do this in end visit sector
    // because the subsector can't be calculated until we have gotten the correct share weights
    // for the technologies.

    calibrateShareWeights( aSector, aPeriod );

    mCurrentSectorName.clear();
}

void CalibrateShareWeightVisitor::startVisitSubsector( const Subsector* aSubsector, const int aPeriod ) {
    // Doing technology calibration in subsector rather than in technology because we will need access
    // to all of the technologies at the same time.

    // First we need to make sure that the technologies have calculated their costs.
    for( auto techContainer : aSubsector->mTechContainers ) {
        techContainer->getNewVintageTechnology( aPeriod )->calcCost( mCurrentRegionName, mCurrentSectorName, aPeriod );
    }

    calibrateShareWeights( aSubsector, aPeriod );
}

void CalibrateShareWeightVisitor::endVisitNestingSubsector( const NestingSubsector* aSubsector, const int aPeriod ) {
    // We need to do this in end visit NestingSubsector because the child subsector
    // can't be calculated until we have gotten the correct share weights from the
    // bottom of the nesting strucutre first.

    calibrateShareWeights( aSubsector, aPeriod );
}


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Core/Exception.h>          // for ArgumentErr, vw_throw, etc
#include <vw/Math/Quaternion.h>         // for Quat, Quaternion
#include <vw/Math/Vector.h>             // for Vector, Vector3, Vector4, etc
#include <vw/Cartography/Datum.h>       // for Datum
#include <vw/FileIO/DiskImageResourceGDAL.h>

#include <asp/Camera/XMLBase.h>
#include <asp/Camera/PeruSatXML.h>
#include <asp/Camera/RPCModel.h>
#include <asp/Camera/XMLBase.h>

#include <xercesc/parsers/XercesDOMParser.hpp>
#include <xercesc/sax/HandlerBase.hpp>
#include <xercesc/sax/SAXException.hpp>
#include <xercesc/sax/SAXParseException.hpp>
#include <xercesc/dom/DOMException.hpp>
#include <xercesc/util/PlatformUtils.hpp>
#include <xercesc/sax/ErrorHandler.hpp>

#include <boost/algorithm/string/predicate.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include <iomanip>

using namespace vw;
using namespace vw::cartography;
using namespace xercesc;

using asp::XmlUtils::get_node;
using asp::XmlUtils::cast_xmlch;

namespace asp {

DOMElement* PeruSatXML::open_xml_file(std::string const& xml_path) {

  // Check if the file actually exists and throw a user helpful file.
  if (!boost::filesystem::exists(xml_path))
    vw_throw(ArgumentErr() << "XML file \"" << xml_path << "\" does not exist.");

  std::string error_prefix = "XML file \"" + xml_path + "\" is invalid.\nException message is: \n";
  std::string err_message  = ""; // Filled in later on error

  try{
    // Set up the XML parser if we have not already done so
    if (!m_parser.get()) {
      m_parser.reset(new XercesDOMParser());
      m_err_handler.reset(new HandlerBase());
      m_parser->setValidationScheme(XercesDOMParser::Val_Always);
      m_parser->setDoNamespaces(true);
      m_parser->setErrorHandler(m_err_handler.get());
    }

    // Load the XML file
    m_parser->parse(xml_path.c_str());
    DOMDocument* doc  = m_parser->getDocument();
    DOMElement * root = doc->getDocumentElement();
    return root;

  } catch (const XMLException& toCatch) {
    char* message = XMLString::transcode(toCatch.getMessage());
    err_message = error_prefix + message;
    XMLString::release(&message);
  } catch (const DOMException& toCatch) {
    char* message = XMLString::transcode(toCatch.msg);
    err_message = error_prefix + message;
    XMLString::release(&message);
  } catch (const SAXException& toCatch) {
    char* message = XMLString::transcode(toCatch.getMessage());
    err_message = error_prefix + message;
    XMLString::release(&message);
  } catch (const std::exception& e) {
    err_message = error_prefix + e.what();
  } catch (...) {
    err_message = "Unrecognized error in XML file \"" + xml_path + "\"\n";
  }
  vw_throw(ArgumentErr() << err_message); // Only get here on error

  return 0;
}

void PeruSatXML::read_xml(std::string const& xml_path) {
  DOMElement * root = open_xml_file(xml_path);
  parse_xml(root);
}

void PeruSatXML::parse_xml(xercesc::DOMElement* root) {

  xercesc::DOMElement* metadata_id = get_node<DOMElement>(root, "Metadata_Identification");

  xercesc::DOMElement* metadata_profile = get_node<DOMElement>(metadata_id, "METADATA_PROFILE");

  std::string sensor_name(XMLString::transcode(metadata_profile->getTextContent()));
  std::string expected_name = "PER1_SENSOR";
  if (sensor_name != expected_name)
    vw_throw(ArgumentErr() << "Incorrect sensor name. Expected: "
             << expected_name << " but got: " << sensor_name << ".\n");

  xercesc::DOMElement* raster_data = get_node<DOMElement>(root, "Raster_Data");
  read_image_size(raster_data);

  // Dig some levels down
  xercesc::DOMElement* geometric_data = get_node<DOMElement>(root, "Geometric_Data");
  xercesc::DOMElement* refined_model = get_node<DOMElement>(geometric_data, "Refined_Model");

  xercesc::DOMElement* time = get_node<DOMElement>(refined_model, "Time");
  read_times(time);

  xercesc::DOMElement* ephemeris = get_node<DOMElement>(refined_model, "Ephemeris");
  read_ephemeris(ephemeris);

  xercesc::DOMElement* attitudes = get_node<DOMElement>(refined_model, "Attitudes");
  read_attitudes(attitudes);

  xercesc::DOMElement* geom_calib  = get_node<DOMElement>(refined_model, "Geometric_Calibration");
  xercesc::DOMElement* instr_calib = get_node<DOMElement>(geom_calib,    "Instrument_Calibration");
  xercesc::DOMElement* band_calib  = get_node<DOMElement>(instr_calib,   "Band_Calibration");
  xercesc::DOMElement* look_angles = get_node<DOMElement>(band_calib,    "Polynomial_Look_Angles");
  read_look_angles(look_angles);

  xercesc::DOMElement* instr_biases = get_node<DOMElement>(instr_calib,   "Instrument_Biases");
  read_instr_biases(instr_biases);

  xercesc::DOMElement* use_area = get_node<DOMElement>(geometric_data, "Use_Area");
  xercesc::DOMElement* geom_values = get_node<DOMElement>(use_area,
                                                          "Located_Geometric_Values");
  read_center_data(geom_values);
}

void PeruSatXML::read_image_size(xercesc::DOMElement* raster_data_node) {
  xercesc::DOMElement* raster_dims_node = get_node<DOMElement>(raster_data_node,
                                                                 "Raster_Dimensions");

  cast_xmlch(get_node<DOMElement>(raster_dims_node, "NROWS")->getTextContent(), m_image_size[1]);
  cast_xmlch(get_node<DOMElement>(raster_dims_node, "NCOLS")->getTextContent(), m_image_size[0]);
}

void PeruSatXML::read_times(xercesc::DOMElement* time) {
  xercesc::DOMElement* time_range = get_node<DOMElement>(time, "Time_Range");

  std::string start_time_str;
  cast_xmlch(get_node<DOMElement>(time_range, "START")->getTextContent(), start_time_str);
  bool is_start_time = true;
  m_start_time = PeruSatXML::convert_time(start_time_str, is_start_time);

  xercesc::DOMElement* time_stamp = get_node<DOMElement>(time, "Time_Stamp");
  cast_xmlch(get_node<DOMElement>(time_stamp, "LINE_PERIOD")->getTextContent(), m_line_period);
}

void PeruSatXML::read_ephemeris(xercesc::DOMElement* ephemeris) {

  // Reset data storage
  m_positions.clear();
  m_velocities.clear();

  xercesc::DOMElement* point_list = get_node<DOMElement>(ephemeris, "Point_List");

  // Pick out the "Point" nodes
  DOMNodeList* children = point_list->getChildNodes();
  for (XMLSize_t i = 0; i < children->getLength(); i++) {

    // Check child node type
    DOMNode* child = children->item(i);
    if (child->getNodeType() != DOMNode::ELEMENT_NODE)
      continue;

    // Check the node name
    DOMElement* curr_element = dynamic_cast<DOMElement*>(child);
    std::string tag(XMLString::transcode(curr_element->getTagName()));
    if (tag.find("Point") == std::string::npos)
      continue;

    // Get the three sub-nodes
    std::string time_str, position_str, velocity_str;
    Vector3 position_vec, velocity_vec;

    cast_xmlch(get_node<DOMElement>(curr_element, "LOCATION_XYZ")->getTextContent(), position_str);
    cast_xmlch(get_node<DOMElement>(curr_element, "VELOCITY_XYZ")->getTextContent(), velocity_str);
    cast_xmlch(get_node<DOMElement>(curr_element, "TIME")->getTextContent(),         time_str);

    bool is_start_time = false;
    double time = PeruSatXML::convert_time(time_str, is_start_time);

    std::string delimiters(",\t ");
    position_vec = str_to_vec<Vector3>(position_str, delimiters);
    velocity_vec = str_to_vec<Vector3>(velocity_str, delimiters);

    m_positions.push_back(std::pair<double, Vector3>(time, position_vec));
    m_velocities.push_back(std::pair<double, Vector3>(time, velocity_vec));
  } // End loop through points

}

void PeruSatXML::read_attitudes(xercesc::DOMElement* attitudes) {

  // Reset data storage
  m_poses.clear();

  xercesc::DOMElement* quaternion_list = get_node<DOMElement>(attitudes, "Quaternion_List");

  // Pick out the "Quaternion" nodes
  DOMNodeList* children = quaternion_list->getChildNodes();
  for (XMLSize_t i = 0; i < children->getLength(); i++) {

    // Check child node type
    DOMNode* child = children->item(i);
    if (child->getNodeType() != DOMNode::ELEMENT_NODE)
      continue;

    // Check the node time
    DOMElement* curr_element = dynamic_cast<DOMElement*>(child);
    std::string tag(XMLString::transcode(curr_element->getTagName()));
    if (tag.find("Quaternion") == std::string::npos)
      continue;

    // Parse the time and

    std::pair<double, vw::Quaternion<double>> data;
    std::string time_str;
    cast_xmlch(get_node<DOMElement>(curr_element, "TIME")->getTextContent(), time_str);

    bool is_start_time = false;
    data.first = PeruSatXML::convert_time(time_str, is_start_time);

    double w, x, y, z;
    cast_xmlch(get_node<DOMElement>(curr_element, "Q0")->getTextContent(), w);
    cast_xmlch(get_node<DOMElement>(curr_element, "Q1")->getTextContent(), x);
    cast_xmlch(get_node<DOMElement>(curr_element, "Q2")->getTextContent(), y);
    cast_xmlch(get_node<DOMElement>(curr_element, "Q3")->getTextContent(), z);
    data.second = vw::Quaternion<double>(w, x, y, z);

    // Normalize the quaternions to remove any inaccuracy due to the
    // limited precision used to save them on disk.
    data.second = normalize(data.second);

    m_poses.push_back(data);
  } // End loop through attitudes
}

void PeruSatXML::read_look_angles(xercesc::DOMElement* look_angles) {
  std::string delimiters(",\t ");
  std::string tan_psi_x_str;
  cast_xmlch(get_node<DOMElement>(look_angles, "LINE_OF_SIGHT_TANPSIX")->getTextContent(),
             tan_psi_x_str);
  m_tan_psi_x = str_to_vec<Vector2>(tan_psi_x_str, delimiters);

  std::string tan_psi_y_str;
  cast_xmlch(get_node<DOMElement>(look_angles, "LINE_OF_SIGHT_TANPSIY")->getTextContent(),
             tan_psi_y_str);
  m_tan_psi_y = str_to_vec<Vector2>(tan_psi_y_str, delimiters);
}

void PeruSatXML::read_instr_biases(xercesc::DOMElement* instr_biases) {

  double w, x, y, z;
  cast_xmlch(get_node<DOMElement>(instr_biases, "Q0")->getTextContent(), w);
  cast_xmlch(get_node<DOMElement>(instr_biases, "Q1")->getTextContent(), x);
  cast_xmlch(get_node<DOMElement>(instr_biases, "Q2")->getTextContent(), y);
  cast_xmlch(get_node<DOMElement>(instr_biases, "Q3")->getTextContent(), z);

  m_instrument_biases = vw::Quaternion<double>(w, x, y, z);
}

void PeruSatXML::read_center_data(xercesc::DOMElement* geom_values) {

  std::string center_time_str;
  cast_xmlch(get_node<DOMElement>(geom_values, "TIME")->getTextContent(), center_time_str);
  cast_xmlch(get_node<DOMElement>(geom_values, "COL")->getTextContent(), m_center_col);
  cast_xmlch(get_node<DOMElement>(geom_values, "ROW")->getTextContent(), m_center_row);

  bool is_start_time = false;
  center_time = PeruSatXML::convert_time(center_time_str, is_start_time);

  // Convert from 1-based to 0-based indices.
  m_center_col -= 1.0;
  m_center_row -= 1.0;
}

// Converts a time from string to double precision value measured in seconds
// relative to the start time.
// Input strings look like this: 2008-03-04T12:31:03.08191Z.
double PeruSatXML::convert_time(std::string const& s, bool is_start_time) {

  if (!is_start_time && !m_start_time_is_set)
    vw::vw_throw(vw::ArgumentErr()
                 << "Must set the start time before doing time conversions.\n");

  try{
    // Replace the T with a space so the default Boost function can
    // parse the time.
    std::string s2 = s;
    boost::replace_all(s2, "T", " ");

    // Ensure there are exactly 6 digits for the millisecond or else
    // Boost will complain.
    s2 = fix_millisecond(s2);

    boost::posix_time::ptime time = boost::posix_time::time_from_string(s2);

    if (is_start_time) {
      m_start_time_is_set = true;
      m_start_time_stamp = time;
    }

    boost::posix_time::time_duration delta(time - m_start_time_stamp);
    return delta.total_microseconds() / 1.0e+6;

  }catch(...){
    vw::vw_throw(vw::ArgumentErr() << "Failed to parse time from string: " << s << "\n");
  }
  return -1.0; // Never reached
}

// Find the time at each line (line starts from 0) by multiplying by
// the period.  All times are relative to the starting time (see the
// convert_time() function).

// Note: PeruSat also has the center time, under Located_Geometric_Values,
// and that corresponds to line (num_lines - 1)/2.0 as expected. Yet PeruSat
// also provides there a center row, but that one is wrong and not equal
// to (num_lines - 1)/2.0.
vw::camera::LinearTimeInterpolation PeruSatXML::setup_time_func() const {
  return vw::camera::LinearTimeInterpolation(m_start_time, m_line_period);
}

// The position is already in GCC, so just pack into a function.
// - Currently this is identical to the velocity function, but this may change later.
vw::camera::LagrangianInterpolation PeruSatXML::setup_position_func
(vw::camera::LinearTimeInterpolation const& time_func) const {

  // Sanity check, we should be able to find the position for each image line
  size_t num_lines           = m_image_size[1];
  double first_line_time     = time_func(0);
  double last_line_time      = time_func(num_lines - 1.0);
  double num_positions       = m_positions.size();
  double position_start_time = m_positions.front().first;
  double position_stop_time  = m_positions.back().first;
  double position_delta_t    = (position_stop_time - position_start_time) / (num_positions - 1.0);

  if (position_start_time > first_line_time || position_stop_time < last_line_time)
    vw_throw(ArgumentErr() << "The position timestamps do not fully span the "
             << "range of times for the image lines.");

  // Use Lagrange interpolation with degree 3 polynomials with 4
  // points in piecewise manner out of 5 provided in the xml
  // file. This is what the doc recommends.
  // TODO(oalexan1): Implement Lagrange interpolation of even degree
  // using an odd number of points, and see if using all 5 points at
  // once is any better.
  const int INTERP_RADII = 2;
  std::vector<double>  time_vec;
  std::vector<Vector3> position_vec;

  // Loop through the positions and extract values
  int index = 0;
  for (auto iter = m_positions.begin(); iter != m_positions.end(); iter++) {
    time_vec.push_back(iter->first);
    position_vec.push_back(iter->second);

    // Sanity check. The times at which the positions are given must
    // be uniformly distributed.
    if (index > 0) {
      double err = std::abs(time_vec[index] - time_vec[index - 1] - position_delta_t)
        / position_delta_t;
      if (err > 1.0e-6)
        vw_throw(ArgumentErr() << "The position timestamps are not uniformly distributed.");
    }

    index++;
  }

  // More generic method for variable time intervals
  // return vw::camera::LagrangianInterpolationVarTime(position_vec, time_vec, INTERP_RADII);

  // A faster method for when we know the time delta is constant
  return vw::camera::LagrangianInterpolation(position_vec, position_start_time,
                                             position_delta_t, position_stop_time, INTERP_RADII);
}

// Velocities are the sum of inertial velocities and the instantaneous
//  Earth rotation.

// The velocity is already in GCC, so just pack into a function.
vw::camera::LagrangianInterpolation PeruSatXML::setup_velocity_func
(vw::camera::LinearTimeInterpolation const& time_func) const {

  // Sanity check, we should be able to find the velocity for each image line
  size_t num_lines           = m_image_size[1];
  double first_line_time     = time_func(0);
  double last_line_time      = time_func(num_lines - 1.0);
  double num_velocities      = m_velocities.size();
  double velocity_start_time = m_velocities.front().first;
  double velocity_stop_time  = m_velocities.back().first;
  double velocity_delta_t    = (velocity_stop_time - velocity_start_time) / (num_velocities - 1.0);

  if (velocity_start_time > first_line_time || velocity_stop_time < last_line_time)
    vw_throw(ArgumentErr() << "The velocity timestamps do not fully span the "
             << "range of times for the image lines.");

  if (velocity_start_time > first_line_time || velocity_stop_time < last_line_time)
    vw_throw(ArgumentErr() << "The velocity timestamps do not fully span the "
             << "range of times for the image lines.");

  // See note when the position function was set up earlier.
  const int INTERP_RADII = 2;
  std::vector<double>  time_vec;
  std::vector<Vector3> velocity_vec;

  // Loop through the velocities and extract values
  int index = 0;
  for (auto iter = m_velocities.begin(); iter != m_velocities.end(); iter++) {
    time_vec.push_back(iter->first);
    velocity_vec.push_back(iter->second);

    // Sanity check. The times at which the velocitys are given must
    // be uniformly distributed.
    if (index > 0) {
      double err = std::abs(time_vec[index] - time_vec[index - 1] - velocity_delta_t)
        / velocity_delta_t;
      if (err > 1.0e-6)
        vw_throw(ArgumentErr() << "The velocity timestamps are not uniformly distributed.");
    }

    index++;
  }

  // More generic method for variable time intervals
  //return vw::camera::LagrangianInterpolationVarTime(velocity_vec, time_vec, INTERP_RADII);

  // A faster method for when we know the time delta is constant
  return vw::camera::LagrangianInterpolation(velocity_vec, velocity_start_time,
                                             velocity_delta_t, velocity_stop_time, INTERP_RADII);
}

// Put the timestamps and poses in vectors and form the pose
// interpolation object.

// TODO(oalexan1): See if using bicubic pose interpolation (as the
// SPOT-6 manual suggests) is better than the bilinear interpolation
// used now.
vw::camera::SLERPPoseInterpolation PeruSatXML::setup_pose_func
  (vw::camera::LinearTimeInterpolation const& time_func) const {

  size_t num_lines           = m_image_size[1];
  double first_line_time     = time_func(0);
  double last_line_time      = time_func(num_lines - 1.0);

  double num_poses           = m_poses.size();
  double pose_start_time     = m_poses.front().first;
  double pose_stop_time      = m_poses.back().first;
  double pose_delta_t        = (pose_stop_time - pose_start_time) / (num_poses - 1.0);

  if (pose_start_time > first_line_time || pose_stop_time < last_line_time)
    vw_throw(ArgumentErr() << "The quaternion timestamps do not fully span the "
             << "range of times for the image lines.");

  std::vector<vw::Quaternion<double>> pose_vec(num_poses);
  std::vector<double>  time_vec(num_poses);
  int index = 0;
  for (auto iter = m_poses.begin(); iter != m_poses.end(); iter++) {
    time_vec[index] = iter->first;
    pose_vec[index] = iter->second;

    // Sanity check. The times at which the quaternions are given must
    // be uniformly distributed. The quaternions are sampled more
    // densely than the positions and velocities, so we tolerate
    // a bit more divergence from uniform sampling.
    if (index > 0) {
      double err = std::abs(time_vec[index] - time_vec[index - 1] - pose_delta_t) / pose_delta_t;
      if (err > 0.01)
        vw_throw(ArgumentErr() << "The quaternion timestamps are not uniformly distributed.");
    }

    index++;
  }

  // Using splines for pose interpolation changed the DEM heights on
  // the order of 2 cm, so it appears not to make a difference.
  bool use_splines = false;
  double min_time = time_vec.front();
  return vw::camera::SLERPPoseInterpolation(pose_vec, min_time, pose_delta_t, use_splines);
}

// Boost does not like a time string such as "2017-12-07 15:36:40.90795Z"
// because it expects precisely 6 digits after the dot. Fix that.
std::string PeruSatXML::fix_millisecond(std::string const& in_str) {

  std::string out_str = "";
  bool found_dot = false;
  int num_digits_after_dot = 0;
  for (size_t it = 0; it < in_str.size(); it++) {

    if (it + 1 < in_str.size()) {
      // Not yet at the last character

      if (in_str[it] == '.') {
        // Found the dot
        found_dot = true;
        out_str += in_str[it];
        continue;
      }

      if (!found_dot) {
        // Not at the dot yet
        out_str += in_str[it];
        continue;
      }

      // After the dot
      if (num_digits_after_dot < 6) {
        out_str += in_str[it];
        num_digits_after_dot++;
      }
      continue;
    }

    // At the last character
    if (in_str[it] >= '0' && in_str[it] <= '9') {
      // The last character is a digit, just append it
      if (num_digits_after_dot < 6) {
        out_str += in_str[it];
        num_digits_after_dot++;
      }

      // See if to append more
      while (num_digits_after_dot < 6) {
        out_str += "0";
        num_digits_after_dot++;
      }

    } else {

      // The last character is not a digit, it is likely a "Z"
      while (num_digits_after_dot < 6) {
        // Append zeros
        out_str += "0";
        num_digits_after_dot++;
      }

      // Append the last character, whatever it is
      out_str += in_str[it];
    }

  } // End iterating over characters

  return out_str;
}

} // end namespace asp