#ifndef LUNAR_TYPE_HPP
#define LUNAR_TYPE_HPP

#include "lunar_common.hpp"
#include "lunar_parser.hpp"

#include <assert.h>

#include <map>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include <boost/bimap/bimap.hpp>
#include <boost/bimap/unordered_set_of.hpp>

#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index_container.hpp>

namespace lunar {

struct type_id {
    std::string m_path;
    std::string m_id;

    void print() const {
        std::cout << "{\"path\":\"" << m_path << "\",\"id\":\"" << m_id
                  << "\"}";
    }

    bool operator==(const type_id &rhs) const {
        return m_path == rhs.m_path && m_id == rhs.m_id;
    }

    bool operator!=(const type_id &rhs) const { return !(*this == rhs); }
};

} // namespace lunar

namespace std {

template <> struct std::hash<lunar::type_id> {
  public:
    size_t operator()(const lunar::type_id &data) const {
        std::size_t seed = 0;
        boost::hash_combine(seed, data.m_path);
        boost::hash_combine(seed, data.m_id);
        return seed;
    }
};

} // namespace std

namespace lunar {

struct ast;
struct ast_kind;
struct ast_type;
struct ast_class;
struct ast_instance;
struct ast_pred;
struct ast_defun;
struct ast_interface;
struct ast_expr;
struct ast_expr_id;
struct ast_binexpr;
struct ast_userdef;
class module;
class parser;
class pred;
class defun;

class substitution;

// a kind is the type of a type constructor or a higher order type operator
// e.g.
//   *
//   * -> *
//   * -> (* -> *)
class kind {
  public:
    kind() {}
    virtual ~kind(){};

    static uint16_t num_args(const ast_kind *ptr);

    virtual void print() = 0;
    virtual std::string to_str() = 0;

    bool m_is_star;
};

typedef std::shared_ptr<kind> shared_kind;

// normal type
class star : public kind {
  public:
    star() { m_is_star = true; }
    virtual ~star() {}
    virtual void print() { std::cout << "*"; }
    virtual std::string to_str() { return "*"; }
};

typedef std::shared_ptr<star> shared_star;

// higher order type
class kfun : public kind {
  public:
    kfun() { m_is_star = false; }
    virtual ~kfun() {}

    virtual void print() {
        m_left->print();
        std::cout << " -> ";
        m_right->print();
    }

    virtual std::string to_str() {
        auto left = m_left->to_str();
        auto right = m_right->to_str();
        return left + " -> " + right;
    }

    shared_kind m_left;
    shared_kind m_right;
};

typedef std::shared_ptr<kfun> shared_kfun;

int cmp_kind(const kind *lhs, const kind *rhs);

// type
class type {
  public:
    type() {}
    virtual ~type(){};

    virtual void print() = 0;
    virtual std::string to_str() = 0;

    enum subtype {
        TYPE_CONST, // constant type
        TYPE_VAR,   // type variable
        TYPE_APP,   // higher order type application
    };

    subtype m_subtype;

    static std::shared_ptr<type> make(const module *ptr_mod,
                                      const ast_type *ptr);

    virtual shared_kind get_kind() const = 0;
};

typedef std::shared_ptr<type> shared_type;

bool eq_type(type *lhs, type *rhs);

// constant type
// e.g.
//   num : *
//   vec : * -> *
class type_const : public type {
  public:
    virtual ~type_const() {}
    virtual void print();
    virtual std::string to_str();

    bool operator==(const type_const &lhs) const {
        return m_id == lhs.m_id &&
               cmp_kind(m_kind.get(), lhs.m_kind.get()) == 0;
    }

    bool operator!=(const type_const &lhs) const { return !(*this == lhs); }

    virtual shared_kind get_kind() const { return m_kind; }
    const type_id &get_id() { return m_id; }

    // id: type name
    // numtargs: the number of type arguments
    static shared_type make(const type_id &id, unsigned int numtargs);

  protected:
    type_const() { m_subtype = TYPE_CONST; }
    type_id m_id; // identifier of type (e.g. num, bool)
    shared_kind m_kind;
};

typedef std::shared_ptr<type_const> shared_type_const;

// type variable
class type_var : public type {
  public:
    type_var(const std::string &id, shared_kind k) : m_id(id), m_kind(k) {
        m_subtype = TYPE_VAR;
    }
    virtual ~type_var() {}

    bool operator==(const type_var &lhs) const {
        return m_id == lhs.m_id &&
               cmp_kind(m_kind.get(), lhs.m_kind.get()) == 0;
    }

    bool operator!=(const type_var &lhs) const { return !(*this == lhs); }

    bool operator<(const type_var &lhs) const {
        int ret = m_id.compare(lhs.m_id);
        if (ret == 0)
            ret = cmp_kind(m_kind.get(), lhs.m_kind.get());
        return ret > 0 ? true : false;
    }

    virtual shared_kind get_kind() const { return m_kind; }
    const std::string &get_id() const { return m_id; }
    void set_id(const std::string &s) { m_id = s; }

    // id: type variable name
    static shared_type make(const std::string &id, unsigned int numtargs);

    // id: type variable name, k: kind
    static shared_type make(const std::string &id, shared_kind k) {
        auto ret = std::shared_ptr<type_var>(new type_var);
        ret->m_id = id;
        ret->m_kind = k;
        return ret;
    }

    virtual void print();
    virtual std::string to_str();

  private:
    type_var() { m_subtype = TYPE_VAR; }
    std::string m_id; // type variable name
    shared_kind m_kind;
};

typedef std::shared_ptr<type_var> shared_type_var;

// application of higher order type
// e.g. (1)
//   struct dict<T1, T2> {...} is denoted as * -> (* -> *)
//   dict<int> is denoted as
//     tapp1.left  = * -> (* -> *)
//     tapp1.right = *
//   dict<int, bool> is denoted as
//     tapp2.left  = tapp1
//     type2.right = *
//
// e.g. (2)
//   struct somedata<T1, T2, T3> {...}
//   somedata<int>
//     tapp1.left  = * -> (* -> (* -> *))
//     tapp1.right = *
//   somedata<int, int>
//     tapp2.left  = tapp1
//     tapp2.right = *
//   somedata<int, int, int>
//     tapp3.left  = tapp2
//     tapp3.right = *
class type_app : public type {
  public:
    virtual ~type_app() {}

    bool operator==(const type_app &lhs) const {
        return eq_type(m_left.get(), m_right.get()) &&
               eq_type(lhs.m_left.get(), lhs.m_right.get());
    }

    // e.g.
    // if
    //   left:  * -> (* -> *)
    //   right: *
    // then output
    //   * -> *
    virtual shared_kind get_kind() const {
        auto k = m_left->get_kind();
        assert(!k->m_is_star);

        auto kf = std::static_pointer_cast<kfun>(k);

        return kf->m_right;
    }
    static shared_type make(shared_type lhs, shared_type rhs);

    // require
    //   m_left->get_kind()->m_left == m_right->get_kind()
    //   m_right->get_kind() == *
    shared_type m_left;
    shared_type m_right;

    virtual void print();
    virtual std::string to_str();
    std::string to_str(bool is_top);

  private:
    type_app() { m_subtype = TYPE_APP; }
};

typedef std::unique_ptr<pred> uniq_pred;

// substitution from (id, kind) to type
// type variable -> type
// e.g.
//   substitution:
//     {(`a : *) -> int}
//   apply:
//     {(`a : *) -> int} `a : * -> int
//   composition:
//     {`b : * -> int} {(`a : *) -> vec (`b : *)} -> {(`a : *) -> vec int}
class substitution {
  public:
    substitution() {}
    virtual ~substitution() {}

    // substitute type variables in the argument
    shared_type apply(shared_type type);
    uniq_pred apply(pred *p);

    std::map<type_var, shared_type> m_subst;
};

typedef std::unique_ptr<substitution> uniq_subst;

// a predicate asserts that m_types are members of a class named m_id
// e.g.
//   num<`a>: class name = num, m_args = [`a]
//   ClassA<`a, `b>: class name = ClassA, m_args = [`a, `b]
class pred {
  public:
    pred() {}
    virtual ~pred() {}

    void print();
    std::string to_str() { return m_id.m_id + "<" + m_arg->to_str() + ">"; }

    static uniq_pred make(const module *ptr_mod, const ast_pred *ptr);

    bool operator==(const pred &rhs) const {
        if (m_id != rhs.m_id)
            return false;

        return eq_type(m_arg.get(), rhs.m_arg.get());
    }

    bool in_hnf() { return hnf(m_arg.get()); }

    bool is_head_var(const std::string &arg) {
        return head_var(m_arg.get(), arg);
    }

    type_id m_id; // class name_id
    shared_type m_arg;

  private:
    static bool hnf(type *p) {
        switch (p->m_subtype) {
        case type::TYPE_VAR:
            return true;
        case type::TYPE_APP: {
            auto ap = (type_app *)p;
            return hnf(ap->m_left.get());
        }
        case type::TYPE_CONST:
            return false;
        }
    }

    static bool head_var(type *p, const std::string &arg) {
        switch (p->m_subtype) {
        case type::TYPE_VAR: {
            auto tv = (type_var *)p;
            if (arg == tv->get_id())
                return true;
            else
                return false;
        }
        case type::TYPE_APP: {
            auto ap = (type_app *)p;
            return head_var(ap->m_left.get(), arg);
        }
        case type::TYPE_CONST:
            return false;
        }
    }
};

typedef boost::bimaps::bimap<boost::bimaps::unordered_set_of<std::string>,
                             boost::bimaps::unordered_set_of<std::string>>
    bimap_ss;

// qualified type
// e.g.
//   qualified class declaration:
//     class ord<`a> require eq<`a>
//   qualified class instance:
//     inst ord<either `a> require ord<'a>
//   qualified type declaration:
//     fn myfun (x : `a, y : `b) : `a require num<`a>, bool<`b>
//     struct data<`a, `b> require num<`a>, bool<`b>
class qual {
  public:
    qual() : m_parent(nullptr) {}
    virtual ~qual() {}

    void print_preds();

    std::vector<uniq_pred> m_preds; // require

    // de Bruijn index <-> original type variable
    bimap_ss m_idx_tvar;

    const ast *m_ast;
    const module *m_module;
    const qual *m_parent;

    std::vector<shared_type> m_tvar_constraint; // type variable constraint

    bool check_kind_constraint(const std::string &id, kind *k, bool &found);
    bool add_constraints(pred *p);
    bool add_constraints(type *p);
    std::string

    // find oritinal type variable from de Bruijn index
    // id: de Bruijn index
    // if found this returns original type variable name,
    // otherwise returns ""
    find_tvar_idx(const std::string &id) const;
};

// qualified type
// function, struct, or union types are denoted by this class
class qual_type : public qual {
  public:
    qual_type() {}
    virtual ~qual_type() {}

    shared_type m_type;
};

typedef std::unique_ptr<qual_type> ptr_qual_type;

// class declaration
class typeclass : public qual {
  public:
    enum ASYCLIC {
        ASYCLIC_NONE,
        ASYCLIC_YES,
        ASYCLIC_NO,
    };
    typeclass() : m_is_asyclic(ASYCLIC_NONE) {}
    virtual ~typeclass() {}

    void print();

    type_id m_id; // class name

    std::string m_arg;                                  // arguments
    std::unordered_map<type_id, ptr_qual_type> m_funcs; // interfaces

    ASYCLIC m_is_asyclic;

    bool apply_super(shared_type arg, std::vector<uniq_pred> &ret,
                     const module *ptr_mod, const ast *ptr_ast);
    bool add_interface(const module *ptr_mod, const std::string &id,
                       ast_interface *ptr_ast);

  private:
    shared_type make_funtype(const module *ptr_mod, ast_interface *ptr_ast);
};

typedef std::unique_ptr<typeclass> uniq_typeclass;

class defun : public qual {
  public:
    defun() {}
    virtual ~defun() {}

    void print();

    static std::unique_ptr<defun>
    make(const module *ptr_mod, const qual *parent, const ast_defun *ast);

    shared_type m_type;
    std::vector<std::pair<std::string, shared_type>> m_args;
    shared_type m_ret;
    std::unordered_map<std::string, shared_type> m_assump;
};

typedef std::unique_ptr<defun> ptr_defun;

// class instance declaration
class inst : public qual {
  public:
    inst() {}
    virtual ~inst() {}

    void print();

    pred m_pred;
    std::unordered_map<std::string, ptr_defun> m_funcs; // interfaces
};

typedef std::unique_ptr<inst> uniq_inst;

class classenv {
  public:
    classenv() {}
    virtual ~classenv() {}

    static std::unique_ptr<classenv> make(const parser &ps);

    void print();

  private:
    struct env {
        uniq_typeclass m_class; // class declaration

        // multiply defined instances are prohibited
        // 0 <= i, j < m_insts.size()
        // s: substitution (mgu)
        // ∀i ∀j ∃s (s m_insts[i].m_args = s m_insts[j].m_args) -> error
        std::vector<uniq_inst> m_insts; // instance declarations
    };

    typedef std::unique_ptr<env> ptr_env;

    // class name -> (class declarations, class instance declarations)
    std::unordered_map<type_id, ptr_env> m_env;

    // function name -> class name
    std::unordered_map<type_id, std::string> m_func2class;

    bool add_class(const module *ptr_mod, const ast_class *ptr);
    bool add_instance(const module *ptr_mod, const ast_instance *ptr_ast);
    bool is_inherit_instance(const pred &p, const module *ptr_mod,
                             const ast *ptr_ast);
    inst *overlap(pred &ptr);
    bool is_asyclic();
    bool is_asyclic(const module *ptr_mod, typeclass *ptr,
                    std::unordered_set<type_id> &visited);

    bool by_super(pred *pd, std::vector<uniq_pred> &ret);
    void by_inst(pred *pd, std::vector<uniq_pred> &ret);
    TRIVAL entail(std::vector<uniq_pred> &ps, pred *pd);
    bool to_hnfs(std::vector<uniq_pred> &ps, std::vector<uniq_pred> &ret,
                 int &idx);
    bool to_hnf(uniq_pred pd, std::vector<uniq_pred> &ret);
    bool simplify(std::vector<uniq_pred> &ps);
    bool reduce(std::vector<uniq_pred> &ps, int &idx);
    bool check_ifs_type(); // check type of interfaces
    uniq_subst mgu_if_type(typeclass *cls, inst *in, const std::string &id,
                           defun *qt);
};

class funcenv {
  public:
    funcenv() {}
    virtual ~funcenv() {}

    void print();

    static std::unique_ptr<funcenv> make(const parser &ps);

    friend class type_infer;
    friend bool typing(classenv &cenv, funcenv &fenv);

  private:
    std::unordered_map<type_id, ptr_defun> m_defuns;
};

namespace mi = boost::multi_index;

class typeenv {
  public:
    typeenv() {}
    virtual ~typeenv() {}

    void print();

    static std::unique_ptr<typeenv> make(const parser &ps);

    friend class type_infer;
    friend bool typing(classenv &cenv, funcenv &fenv);

    struct memv {
        std::string m_id;
        mutable shared_type m_type;

        memv(const std::string &id, shared_type t) : m_id(id), m_type(t) {}
    };

  private:
    struct hashmap {};
    struct seq {};

    typedef mi::multi_index_container<
        memv, mi::indexed_by<
                  mi::hashed_unique<mi::tag<hashmap>,
                                    mi::member<memv, std::string, &memv::m_id>>,
                  mi::random_access<mi::tag<seq>>>>
        dict_mem;

    struct typeinfo {
        qual_type m_type;
        std::vector<shared_type> m_args;
        dict_mem m_members;
        bool m_is_struct;
        const ast_type *m_ast;
        const module *m_module;
    };

    static bool check_tvar(type *ptr,
                           const std::unordered_set<std::string> &tvars);
    static std::shared_ptr<typeinfo> make_typeinfo(const module *ptr_mod,
                                                   const ast_userdef *ptr_ast);

    // return true if info is not recursively defined
    bool check_recursive(const typeinfo &info,
                         std::unordered_set<type_id> &used);

    // return true if ptr was checked whether it is not defined recursively
    bool is_rec_checked(const type_id &id, shared_type ptr);

    std::unordered_map<type_id, std::shared_ptr<typeinfo>> m_types;
    std::unordered_multimap<type_id, shared_type> m_rec_checked;
};

class type_infer {
  public:
    type_infer(defun &fun, classenv &cenv, funcenv &fenv);
    virtual ~type_infer() {}

    bool typing();

  private:
    defun &m_defun;
    classenv &m_classenv;
    funcenv &m_funcenv;
    shared_type m_type;
    shared_type m_ret;
    std::vector<uniq_pred> m_preds;
    uniq_subst m_sbst;
    ast_defun *m_ast;
    uint64_t m_de_bruijn_idx;

    // assumpsion: variable names to types
    std::unordered_map<std::string, std::vector<shared_type>> m_assump;

    // constraints of kind
    std::unordered_map<std::string, shared_kind> m_tvar_constraint;

    // variable names in function
    // storead variable name as de Bruijn index
    std::vector<std::vector<std::string>> m_block_ids;

    // variable names in current scope
    // storead variable name as de Bruijn index
    std::vector<std::vector<std::string>>::reverse_iterator m_ids;

    // de Bruijn index to original variable name
    std::unordered_map<std::string, std::string> m_idx2name;

    // original variable name to de Bruijn index
    std::unordered_map<std::string, std::vector<std::string>> m_name2idx;

    shared_type typing(ast_expr *expr);
    shared_type typing_id(ast_expr_id *expr);
    shared_type typing_dotexpr(ast_binexpr *expr);
    shared_type typing_dotexpr(ast_expr *expr, std::list<std::string> &ids);

    // generate de Bruijn index for var, and returned the generated index
    std::string gensym_for(const std::string &var);

    // return de Bruijn index of var
    // if no variable name is found, return ""
    std::string name2idx(const std::string &var);

    void pop_variables();
};

bool typing(classenv &cenv, funcenv &fenv);

} // namespace lunar

#endif // LUNAR_TYPE_HPP


/**
 *  This file is part of dvo.
 *
 *  Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  dvo is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dvo/core/rgbd_image.h>

#include <assert.h>

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <dvo/core/interpolation.h>

//#include "../util.h"
//#include "../stopwatch.h"

namespace dvo
{
namespace core
{

template<typename T>
static void pyrDownMeanSmooth(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      int x0 = x * 2;
      int x1 = x0 + 1;
      int y0 = y * 2;
      int y1 = y0 + 1;

      out.at<T>(y, x) = (T) ( (in.at<T>(y0, x0) + in.at<T>(y0, x1) + in.at<T>(y1, x0) + in.at<T>(y1, x1)) / 4.0f );
    }
  }
}

template<typename T>
static void pyrDownMeanSmoothIgnoreInvalid(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      int x0 = x * 2;
      int x1 = x0 + 1;
      int y0 = y * 2;
      int y1 = y0 + 1;

      T total = 0;
      int cnt = 0;

      if(std::isfinite(in.at<T>(y0, x0)))
      {
        total += in.at<T>(y0, x0);
        cnt++;
      }

      if(std::isfinite(in.at<T>(y0, x1)))
      {
        total += in.at<T>(y0, x1);
        cnt++;
      }

      if(std::isfinite(in.at<T>(y1, x0)))
      {
        total += in.at<T>(y1, x0);
        cnt++;
      }

      if(std::isfinite(in.at<T>(y1, x1)))
      {
        total += in.at<T>(y1, x1);
        cnt++;
      }

      if(cnt > 0)
      {
        out.at<T>(y, x) = (T) ( total / cnt );
      }
      else
      {
        out.at<T>(y, x) = InvalidDepth;
      }
    }
  }
}

template<typename T>
static void pyrDownMedianSmooth(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  cv::Mat in_smoothed;
  cv::medianBlur(in, in_smoothed, 3);

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      out.at<T>(y, x) = in_smoothed.at<T>(y * 2, x * 2);
    }
  }
}

template<typename T>
static void pyrDownSubsample(const cv::Mat& in, cv::Mat& out)
{
  out.create(cv::Size(in.size().width / 2, in.size().height / 2), in.type());

  for(int y = 0; y < out.rows; ++y)
  {
    for(int x = 0; x < out.cols; ++x)
    {
      out.at<T>(y, x) = in.at<T>(y * 2, x * 2);
    }
  }
}

RgbdImagePyramid::RgbdImagePyramid(RgbdCameraPyramid& camera, const cv::Mat& intensity, const cv::Mat& depth) :
    camera_(camera)
{
  levels_.push_back(camera_.level(0).create(intensity, depth));
}

RgbdImagePyramid::~RgbdImagePyramid()
{
}

void RgbdImagePyramid::compute(const size_t num_levels)
{
  build(num_levels);
}

void RgbdImagePyramid::build(const size_t num_levels)
{
  if(levels_.size() >= num_levels) return;

  // if we already have some levels, we just need to compute the coarser levels
  size_t first = levels_.size();

  for(size_t idx = first; idx < num_levels; ++idx)
  {
    levels_.push_back(camera_.level(idx).create());

    pyrDownMeanSmooth<IntensityType>(levels_[idx - 1]->intensity, levels_[idx]->intensity);
    //pyrDownMeanSmoothIgnoreInvalid<float>(levels[idx - 1].depth, levels[idx].depth);
    pyrDownSubsample<float>(levels_[idx - 1]->depth, levels_[idx]->depth);
    levels_[idx]->initialize();
  }
}

RgbdImage& RgbdImagePyramid::level(size_t idx)
{
  assert(idx < levels_.size());

  return *levels_[idx];
}

double RgbdImagePyramid::timestamp() const
{
  return !levels_.empty() ? levels_[0]->timestamp: 0.0;
}

RgbdCamera::RgbdCamera(size_t width, size_t height, const IntrinsicMatrix& intrinsics) :
    width_(width),
    height_(height),
    intrinsics_(intrinsics)
{
  pointcloud_template_.resize(Eigen::NoChange, width_ * height_);
  int idx = 0;

  for(size_t y = 0; y < height_; ++y)
  {
    for(size_t x = 0; x < width_; ++x, ++idx)
    {
      pointcloud_template_(0, idx) = (x - intrinsics_.ox()) / intrinsics_.fx();
      pointcloud_template_(1, idx) = (y - intrinsics_.oy()) / intrinsics_.fy();
      pointcloud_template_(2, idx) = 1.0;
      pointcloud_template_(3, idx) = 0.0;
    }
  }
}

RgbdCamera::~RgbdCamera()
{
}

size_t RgbdCamera::width() const
{
  return width_;
}

size_t RgbdCamera::height() const
{
  return height_;
}

const dvo::core::IntrinsicMatrix& RgbdCamera::intrinsics() const
{
  return intrinsics_;
}

RgbdImagePtr RgbdCamera::create(const cv::Mat& intensity, const cv::Mat& depth) const
{
  RgbdImagePtr result(new RgbdImage(*this));
  result->intensity = intensity;
  result->depth = depth;
  result->initialize();

  return result;
}

RgbdImagePtr RgbdCamera::create() const
{
  return boost::make_shared<RgbdImage>(*this);
}

bool RgbdCamera::hasSameSize(const cv::Mat& img) const
{
  return img.cols == width_ && img.rows == height_;
}

void RgbdCamera::buildPointCloud(const cv::Mat &depth, PointCloud& pointcloud) const
{
  assert(hasSameSize(depth));

  pointcloud.resize(Eigen::NoChange, width_ * height_);

  const float* depth_ptr = depth.ptr<float>();
  int idx = 0;

  for(size_t y = 0; y < height_; ++y)
  {
    for(size_t x = 0; x < width_; ++x, ++depth_ptr, ++idx)
    {
      pointcloud.col(idx) = pointcloud_template_.col(idx) * (*depth_ptr);
      pointcloud(3, idx) = 1.0;
    }
  }
}

RgbdCameraPyramid::RgbdCameraPyramid(const RgbdCamera& base)
{
  levels_.push_back(boost::make_shared<RgbdCamera>(base));
}

RgbdCameraPyramid::RgbdCameraPyramid(size_t base_width, size_t base_height, const dvo::core::IntrinsicMatrix& base_intrinsics)
{
  levels_.push_back(boost::make_shared<RgbdCamera>(base_width, base_height, base_intrinsics));
}

RgbdCameraPyramid::~RgbdCameraPyramid()
{
}

RgbdImagePyramidPtr RgbdCameraPyramid::create(const cv::Mat& base_intensity, const cv::Mat& base_depth)
{
  return RgbdImagePyramidPtr(new RgbdImagePyramid(*this, base_intensity, base_depth));
}

void RgbdCameraPyramid::build(size_t levels)
{
  size_t start = levels_.size();

  for(size_t idx = start; idx < levels; ++idx)
  {
    RgbdCameraPtr& previous = levels_[idx - 1];

    dvo::core::IntrinsicMatrix intrinsics(previous->intrinsics());
    intrinsics.scale(0.5f);

    levels_.push_back(boost::make_shared<RgbdCamera>(previous->width() / 2, previous->height() / 2, intrinsics));
  }
}

const RgbdCamera& RgbdCameraPyramid::level(size_t level)
{
  build(level + 1);

  return *levels_[level];
}

const RgbdCamera& RgbdCameraPyramid::level(size_t level) const
{
  return *levels_[level];
}
/*
RgbdImage::RgbdImage() :
  camera_(0),
  intensity_requires_calculation_(true),
  depth_requires_calculation_(true),
  pointcloud_requires_build_(true),
  width(0),
  height(0)
{
}
*/
RgbdImage::RgbdImage(const RgbdCamera& camera) :
  camera_(camera),
  intensity_requires_calculation_(true),
  depth_requires_calculation_(true),
  pointcloud_requires_build_(true),
  width(0),
  height(0)
{
}

RgbdImage::~RgbdImage()
{
}

const RgbdCamera& RgbdImage::camera() const
{
  return camera_;
}

void RgbdImage::initialize()
{
  assert(hasIntensity() || hasDepth());

  if(hasIntensity() && hasDepth())
  {
    assert(intensity.size() == depth.size());
  }

  if(hasIntensity())
  {
    assert(intensity.type() == cv::DataType<IntensityType>::type && intensity.channels() == 1);
    width = intensity.cols;
    height = intensity.rows;
  }
  if(hasDepth())
  {
    assert(depth.type() == cv::DataType<DepthType>::type && depth.channels() == 1);
    width = depth.cols;
    height = depth.rows;
  }

  intensity_requires_calculation_ = true;
  depth_requires_calculation_ = true;
  pointcloud_requires_build_ = true;
}

bool RgbdImage::hasIntensity() const
{
  return !intensity.empty();
}

bool RgbdImage::hasRgb() const
{
  return !rgb.empty();
}

bool RgbdImage::hasDepth() const
{
  return !depth.empty();
}

void RgbdImage::calculateDerivatives()
{
  calculateIntensityDerivatives();
  calculateDepthDerivatives();
}

bool RgbdImage::calculateIntensityDerivatives()
{
  if(!intensity_requires_calculation_) return false;

  assert(hasIntensity());

  calculateDerivativeX<IntensityType>(intensity, intensity_dx);
  //calculateDerivativeY<IntensityType>(intensity, intensity_dy);
  calculateDerivativeYSseFloat(intensity, intensity_dy);
  /*
  cv::Mat dy_ref, diff;
  calculateDerivativeY<IntensityType>(intensity, dy_ref);
  cv::absdiff(dy_ref, intensity_dy, diff);
  tracker::util::show("diff", diff);
  cv::waitKey(0);
   */
  intensity_requires_calculation_ = false;
  return true;
}

void RgbdImage::calculateDepthDerivatives()
{
  if(!depth_requires_calculation_) return;

  assert(hasDepth());

  calculateDerivativeX<DepthType>(depth, depth_dx);
  calculateDerivativeY<DepthType>(depth, depth_dy);

  depth_requires_calculation_ = false;
}

template<typename T>
void RgbdImage::calculateDerivativeX(const cv::Mat& img, cv::Mat& result)
{
  result.create(img.size(), img.type());

  for(int y = 0; y < img.rows; ++y)
  {
    for(int x = 0; x < img.cols; ++x)
    {
      int prev = std::max(x - 1, 0);
      int next = std::min(x + 1, img.cols - 1);

      result.at<T>(y, x) = (T) (img.at<T>(y, next) - img.at<T>(y, prev)) * 0.5f;
    }
  }

  //cv::Sobel(img, result, -1, 1, 0, 3, 1.0f / 4.0f, 0, cv::BORDER_REPLICATE);

  // compiler auto-vectorization
  /*
  const float* img_ptr = img.ptr<float>();
  float* result_ptr = result.ptr<float>();

  for(int y = 0; y < img.rows; ++y)
  {
    *result_ptr++ = img_ptr[1] - img_ptr[0];

    for(int x = 1; x < img.cols - 1; ++x, ++img_ptr)
    {
      *result_ptr++ = img_ptr[2] - img_ptr[0];
    }

    *result_ptr++ = img_ptr[1] - img_ptr[0];

    img_ptr++;
  }
   */
}

template<typename T>
void RgbdImage::calculateDerivativeY(const cv::Mat& img, cv::Mat& result)
{
  result.create(img.size(), img.type());

  for(int y = 0; y < img.rows; ++y)
  {
    for(int x = 0; x < img.cols; ++x)
    {
      int prev = std::max(y - 1, 0);
      int next = std::min(y + 1, img.rows - 1);

      result.at<T>(y, x) = (T) (img.at<T>(next, x) - img.at<T>(prev, x)) * 0.5f;
    }
  }
  //cv::Sobel(img, result, -1, 0, 1, 3, 1.0f / 4.0f, 0, cv::BORDER_REPLICATE);

  // compiler auto-vectorization
  /*
  for(int y = 0; y < img.rows; ++y)
  {
    const float* prev_row = img.ptr<float>(std::max(y - 1, 0), 0);
    const float* next_row = img.ptr<float>(std::min(y + 1, img.rows - 1), 0);
    float* cur_row = result.ptr<float>(y, 0);

    for(int x = 0; x < img.cols; ++x)
    {
      *cur_row++ = *next_row++ - *prev_row++;
    }
  }
   */
}

void RgbdImage::buildPointCloud()
{
  if(!pointcloud_requires_build_) return;

  assert(hasDepth());

  camera_.buildPointCloud(depth, pointcloud);

  pointcloud_requires_build_ = false;
}

void RgbdImage::calculateNormals()
{
  if(angles.total() == 0)
  {
    normals = cv::Mat::zeros(depth.size(), CV_32FC4);
    angles.create(depth.size(), CV_32FC1);

    float *angle_ptr = angles.ptr<float>();
    cv::Vec4f *normal_ptr = normals.ptr<cv::Vec4f>();

    int x_max = depth.cols - 1;
    int y_max = depth.rows - 1;

    for(int y = 0; y < depth.rows; ++y)
    {
      for(int x = 0; x < depth.cols; ++x, ++angle_ptr, ++normal_ptr)
      {
        int idx1 = y * depth.cols + std::max(x-1, 0);
        int idx2 = y * depth.cols + std::min(x+1, x_max);
        int idx3 = std::max(y-1, 0) * depth.cols + x;
        int idx4 = std::min(y+1, y_max) * depth.cols + x;

        Eigen::Vector4f::AlignedMapType n(normal_ptr->val);
        n = (pointcloud.col(idx2) - pointcloud.col(idx1)).cross3(pointcloud.col(idx4) - pointcloud.col(idx3));
        n.normalize();

        *angle_ptr = std::abs(n(2));
      }
    }
  }
}

void RgbdImage::buildAccelerationStructure()
{
  if(acceleration.total() == 0)
  {
    calculateDerivatives();
    cv::Mat zeros = cv::Mat::zeros(intensity.size(), intensity.type());
    cv::Mat channels[8] = { intensity, depth, intensity_dx, intensity_dy, depth_dx, depth_dy, zeros, zeros};
    cv::merge(channels, 8, acceleration);
  }
}

void RgbdImage::warpIntensity(const AffineTransform& transformationd, const PointCloud& reference_pointcloud, const IntrinsicMatrix& intrinsics, RgbdImage& result, PointCloud& transformed_pointcloud)
{
  Eigen::Affine3f transformation = transformationd.cast<float>();

  cv::Mat warped_image(intensity.size(), intensity.type());
  cv::Mat warped_depth(depth.size(), depth.type());

  float ox = intrinsics.ox();
  float oy = intrinsics.oy();

  float* warped_intensity_ptr = warped_image.ptr<IntensityType>();
  float* warped_depth_ptr = warped_depth.ptr<DepthType>();

  int outliers = 0;
  int total = 0;
  int idx = 0;

  transformed_pointcloud = transformation * reference_pointcloud;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++idx, ++warped_intensity_ptr, ++warped_depth_ptr)
    {

      const Eigen::Vector4f& p3d = transformed_pointcloud.col(idx);

      if(!std::isfinite(p3d(2)))
      {
        *warped_intensity_ptr = Invalid;
        *warped_depth_ptr = InvalidDepth;
        continue;
      }

      float x_projected = (float) (p3d(0) * intrinsics.fx() / p3d(2) + ox);
      float y_projected = (float) (p3d(1) * intrinsics.fy() / p3d(2) + oy);

      if(inImage(x_projected, y_projected))
      {
        float z = (float) p3d(2);

        *warped_intensity_ptr = Interpolation::bilinearWithDepthBuffer(this->intensity, this->depth, x_projected, y_projected, z);
        *warped_depth_ptr = z;
      }
      else
      {
        *warped_intensity_ptr = Invalid;
        *warped_depth_ptr = InvalidDepth;
        //outliers++;
      }

      //total++;
    }
  }

  result.intensity = warped_image;
  result.depth = warped_depth;
  result.initialize();
}

void RgbdImage::warpDepthForward(const AffineTransform& transformationx, const IntrinsicMatrix& intrinsics, RgbdImage& result, cv::Mat_<cv::Vec3d>& cloud)
{
  Eigen::Affine3d transformation = transformationx.cast<double>();

  cloud = cv::Mat_<cv::Vec3d>(depth.size(), cv::Vec3d(0, 0, 0));
  cv::Mat warped_depth = cv::Mat::zeros(depth.size(), depth.type());
  warped_depth.setTo(InvalidDepth);

  float ox = intrinsics.ox();
  float oy = intrinsics.oy();

  const float* depth_ptr = depth.ptr<float>();
  int outliers = 0;
  int total = 0;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++depth_ptr)
    {
      if(!std::isfinite(*depth_ptr))
      {
        continue;
      }

      float depth = *depth_ptr;
      Eigen::Vector3d p3d((x - ox) * depth / intrinsics.fx(), (y - oy) * depth / intrinsics.fy(), depth);
      Eigen::Vector3d p3d_transformed = transformation * p3d;

      float x_projected = (float) (p3d_transformed(0) * intrinsics.fx() / p3d_transformed(2) + ox);
      float y_projected = (float) (p3d_transformed(1) * intrinsics.fy() / p3d_transformed(2) + oy);

      if(inImage(x_projected, y_projected))
      {
        int yi = (int) y_projected, xi = (int) x_projected;

        if(!std::isfinite(warped_depth.at<DepthType>(yi, xi)) || (warped_depth.at<DepthType>(yi, xi) - 0.05) > depth)
          warped_depth.at<DepthType>(yi, xi) = depth;
      }

      p3d = p3d_transformed;

      total++;
      cloud(y, x) = cv::Vec3d(p3d(0), p3d(1), p3d(2));
    }
  }

  result.depth = warped_depth;
  result.initialize();
}

void RgbdImage::warpIntensityForward(const AffineTransform& transformationx, const IntrinsicMatrix& intrinsics, RgbdImage& result, cv::Mat_<cv::Vec3d>& cloud)
{
  Eigen::Affine3d transformation = transformationx.cast<double>();

  bool identity = transformation.affine().isIdentity(1e-6);

  cloud = cv::Mat_<cv::Vec3d>(intensity.size(), cv::Vec3d(0, 0, 0));
  cv::Mat warped_image = cv::Mat::zeros(intensity.size(), intensity.type());

  float ox = intrinsics.ox();
  float oy = intrinsics.oy();

  const float* depth_ptr = depth.ptr<float>();
  int outliers = 0;
  int total = 0;

  for(size_t y = 0; y < height; ++y)
  {
    for(size_t x = 0; x < width; ++x, ++depth_ptr)
    {
      if(*depth_ptr <= 1e-6f) continue;

      float depth = *depth_ptr;
      Eigen::Vector3d p3d((x - ox) * depth / intrinsics.fx(), (y - oy) * depth / intrinsics.fy(), depth);

      if(!identity)
      {
        Eigen::Vector3d p3d_transformed = transformation * p3d;

        float x_projected = (float) (p3d_transformed(0) * intrinsics.fx() / p3d_transformed(2) + ox);
        float y_projected = (float) (p3d_transformed(1) * intrinsics.fy() / p3d_transformed(2) + oy);

        if(inImage(x_projected, y_projected))
        {
          int xp, yp;
          xp = (int) std::floor(x_projected);
          yp = (int) std::floor(y_projected);

          warped_image.at<IntensityType>(yp, xp) = intensity.at<IntensityType>(y, x);
        }
        else
        {
          outliers++;
        }

        p3d = p3d_transformed;
      }

      total++;
      cloud(y, x) = cv::Vec3d(p3d(0), p3d(1), p3d(2));
    }
  }

  //std::cerr << "warp out: " << outliers << " total: " << total << std::endl;

  if(identity)
  {
    warped_image = intensity;
  }
  else
  {
    //std::cerr << "did warp" << std::endl;
  }

  result.intensity = warped_image;
  result.depth = depth;
  result.initialize();
}

void RgbdImage::warpDepthForwardAdvanced(const AffineTransform& transformation, const IntrinsicMatrix& intrinsics, RgbdImage& result)
{
  assert(hasDepth());

  this->buildPointCloud();

  PointCloud transformed_pointcloud = transformation.cast<float>() * pointcloud;

  cv::Mat warped_depth(depth.size(), depth.type());
  warped_depth.setTo(InvalidDepth);

  float z_factor1 = transformation.rotation()(0, 0) + transformation.rotation()(0, 1) * (intrinsics.fx() / intrinsics.fy());
  float x_factor1 = -transformation.rotation()(2, 0) - transformation.rotation()(2, 1) * (intrinsics.fx() / intrinsics.fy());

  float z_factor2 = transformation.rotation()(1, 1) + transformation.rotation()(1, 0) * (intrinsics.fy() / intrinsics.fx());
  float y_factor2 = -transformation.rotation()(2, 1) - transformation.rotation()(2, 0) * (intrinsics.fy() / intrinsics.fx());

  for(int idx = 0; idx < height * width; ++idx)
  {
    Vector4 p3d = pointcloud.col(idx);
    NumType z = p3d(2);

    if(!std::isfinite(z)) continue;

    int x_length = (int) std::ceil(z_factor1 + x_factor1 * p3d(0) / z) + 1; // magic +1
    int y_length = (int) std::ceil(z_factor2 + y_factor2 * p3d(1) / z) + 1; // magic +1

    Vector4 p3d_transformed = transformed_pointcloud.col(idx);
    NumType z_transformed = p3d_transformed(2);

    int x_projected = (int) std::floor(p3d_transformed(0) * intrinsics.fx() / z_transformed + intrinsics.ox());
    int y_projected = (int) std::floor(p3d_transformed(1) * intrinsics.fy() / z_transformed + intrinsics.oy());

    // TODO: replace inImage(...) checks, with max(..., 0) on initial value of x_, y_ and  min(..., width/height) for their respective upper bound
    //for (int y_ = y_projected; y_ < y_projected + y_length; y_++)
    //  for (int x_ = x_projected; x_ < x_projected + x_length; x_++)

    int x_begin = std::max(x_projected, 0);
    int y_begin = std::max(y_projected, 0);
    int x_end = std::min(x_projected + x_length, (int) width);
    int y_end = std::min(y_projected + y_length, (int) height);

    for (int y = y_begin; y < y_end; ++y)
    {
      DepthType* v = warped_depth.ptr<DepthType>(y, x_begin);

      for (int x = x_begin; x < x_end; ++x, ++v)
      {
        if(!std::isfinite(*v) || (*v) > z_transformed)
        {
          (*v) = (DepthType) z_transformed;
        }
      }
    }
  }

  result.depth = warped_depth;
  result.initialize();
}

bool RgbdImage::inImage(const float& x, const float& y) const
{
  return x >= 0 && x < width && y >= 0 && y < height;
}

} /* namespace core */
} /* namespace dvo */


// Copyright (c) 2012 Andre Martins
// All Rights Reserved.
//
// This file is part of AD3 2.1.
//
// AD3 2.1 is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// AD3 2.1 is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with AD3 2.1.  If not, see <http://www.gnu.org/licenses/>.

#include <cmath>
#include "GenericFactor.h"
#include "Utils.h"
#define EIGEN
#ifdef EIGEN
#include <Eigen/Eigenvalues>
#else
#include "lapacke/lapacke.h"
#endif
#include <limits>

namespace AD3 {

#ifdef PRINT_INVERSION_STATS
static int num_inversions_from_scratch = 0;
static int num_inversions_incremental = 0;
static int num_eigenvalue_computations = 0;
#endif

void
GenericFactor::ClearActiveSet()
{
    for (size_t j = 0; j < active_set_.size(); ++j) {
        DeleteConfiguration(active_set_[j]);
    }
    active_set_.clear();
}

void
GenericFactor::InitActiveSet(Configuration configuration)
{
    init_configuration_ = configuration;
}

void
GenericFactor::InitActiveSetFromScores(
    const vector<double>& variable_log_potentials,
    const vector<double>& additional_log_potentials)
{
    Configuration configuration = CreateConfiguration();

    double value;
    Maximize(variable_log_potentials,
             additional_log_potentials,
             configuration,
             &value);
    InitActiveSet(configuration);
}

bool
GenericFactor::InvertAfterInsertion(const vector<Configuration>& active_set,
                                    const Configuration& inserted_element)
{
#ifdef PRINT_INVERSION_STATS
    ++num_inversions_incremental;
    if (0 == num_inversions_incremental % 10000) {
        cout << "Number of incremental inversions: "
             << num_inversions_incremental << endl;
        cout << "Number of standard inversions: " << num_inversions_from_scratch
             << endl;
        cout << "Number of eigenvalue computations: "
             << num_eigenvalue_computations << endl;
    }
#endif

    vector<double> inverse_A = inverse_A_;
    int size_A = active_set.size() + 1;
    vector<double> r(size_A);

    r[0] = 1.0;
    for (size_t i = 0; i < active_set.size(); ++i) {
        // Count how many variable values the new assignment
        // have in common with the i-th assignment.
        r[i + 1] = CountCommonValuesAdapt(active_set[i], inserted_element);
    }

    double r0 = CountCommonValuesAdapt(inserted_element, inserted_element);
    double s = r0;
    for (int i = 0; i < size_A; ++i) {
        if (r[i] == 0.0)
            continue;
        s -= r[i] * r[i] * inverse_A[i * size_A + i];
        for (int j = i + 1; j < size_A; ++j) {
            if (r[j] == 0.0)
                continue;
            s -= 2 * r[i] * r[j] * inverse_A[i * size_A + j];
        }
    }

    if (NEARLY_ZERO_TOL(s, 1e-9)) {
        if (verbosity_ > 2) {
            cout
              << "Warning: updated matrix will become singular after insertion."
              << endl;
        }
        return false;
    }

    double invs = 1.0 / s;
    vector<double> d(size_A, 0.0);
    for (int i = 0; i < size_A; ++i) {
        if (r[i] == 0.0)
            continue;
        for (int j = 0; j < size_A; ++j) {
            d[j] += inverse_A[i * size_A + j] * r[i];
        }
    }

    int size_A_after = size_A + 1;
    inverse_A_.resize(size_A_after * size_A_after);
    for (int i = 0; i < size_A; ++i) {
        for (int j = 0; j < size_A; ++j) {
            inverse_A_[i * size_A_after + j] =
              inverse_A[i * size_A + j] + invs * d[i] * d[j];
        }
        inverse_A_[i * size_A_after + size_A] = -invs * d[i];
        inverse_A_[size_A * size_A_after + i] = -invs * d[i];
    }
    inverse_A_[size_A * size_A_after + size_A] = invs;

    return true;
}

void
GenericFactor::InvertAfterRemoval(const vector<Configuration>& active_set,
                                  int removed_index)
{
#ifdef PRINT_INVERSION_STATS
    ++num_inversions_incremental;
    if (0 == num_inversions_incremental % 10000) {
        cout << "Number of incremental inversions: "
             << num_inversions_incremental << endl;
        cout << "Number of standard inversions: " << num_inversions_from_scratch
             << endl;
        cout << "Number of eigenvalue computations: "
             << num_eigenvalue_computations << endl;
    }
#endif

    vector<double> inverse_A = inverse_A_;
    int size_A = active_set.size() + 1;
    vector<double> r(size_A);

    ++removed_index; // Index in A has an offset of 1.
    double invs = inverse_A[removed_index * size_A + removed_index];
    assert(!NEARLY_ZERO_TOL(
      invs,
      1e-12)); // TODO: Make this tolerance depend on the scale of the data.
    double s = 1.0 / invs;
    vector<double> d(size_A - 1, 0.0);
    int k = 0;
    for (int i = 0; i < size_A; ++i) {
        if (i == removed_index)
            continue;
        d[k] = -s * inverse_A[removed_index * size_A + i];
        ++k;
    }

    int size_A_after = size_A - 1;
    inverse_A_.resize(size_A_after * size_A_after);
    k = 0;
    for (int i = 0; i < size_A; ++i) {
        if (i == removed_index)
            continue;
        int l = 0;
        for (int j = 0; j < size_A; ++j) {
            if (j == removed_index)
                continue;
            inverse_A_[k * size_A_after + l] =
              inverse_A[i * size_A + j] - invs * d[k] * d[l];
            ++l;
        }
        ++k;
    }
}

// Compute Mnz'*Mnz
void
GenericFactor::ComputeActiveSetSimilarities(
  const vector<Configuration>& active_set,
  vector<double>* similarities)
{
    size_t size = active_set.size();

    // Compute similarity matrix.
    similarities->resize(size * size);
    (*similarities)[0] = 0.0;
    for (size_t i = 0; i < size; ++i) {
        (*similarities)[i * size + i] =
          CountCommonValuesAdapt(active_set[i], active_set[i]);
        for (size_t j = i + 1; j < size; ++j) {
            // Count how many variable values the i-th and j-th
            // assignments have in common.
            double num_common_values =
              CountCommonValuesAdapt(active_set[i], active_set[j]);
            (*similarities)[i * size + j] = num_common_values;
            (*similarities)[j * size + i] = num_common_values;
        }
    }
}

// Compute eigendecomposition of M'*M.
// Remark: overwrite similarities with the eigenvectors.
void
GenericFactor::EigenDecompose(vector<double>* similarities,
                              vector<double>* eigenvalues)
{
#ifdef PRINT_INVERSION_STATS
    ++num_eigenvalue_computations;
    if (0 == num_eigenvalue_computations % 10000) {
        cout << "Number of incremental inversions: "
             << num_inversions_incremental << endl;
        cout << "Number of standard inversions: " << num_inversions_from_scratch
             << endl;
        cout << "Number of eigenvalue computations: "
             << num_eigenvalue_computations << endl;
    }
#endif

    int size =
      static_cast<int>(sqrt(static_cast<double>(similarities->size())));
#ifdef EIGEN
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
    Eigen::MatrixXd sim(size, size);
    int t = 0;
    for (int i = 0; i < size; ++i) {
        for (int j = 0; j < size; ++j) {
            sim(i, j) = (*similarities)[t];
            ++t;
        }
    }
    es.compute(sim);
    const Eigen::VectorXd& eigvals = es.eigenvalues();
    eigenvalues->resize(size);
    for (int i = 0; i < size; ++i) {
        (*eigenvalues)[i] = eigvals[i];
    }
    const Eigen::MatrixXd& eigvectors = es.eigenvectors().transpose();
    t = 0;
    for (int i = 0; i < size; ++i) {
        for (int j = 0; j < size; ++j) {
            (*similarities)[t] = eigvectors(i, j);
            ++t;
        }
    }
#else
    lapack_int info;
    eigenvalues->resize(size);
    info = LAPACKE_dsyev(LAPACK_COL_MAJOR,
                         'V',
                         'U',
                         size,
                         &(*similarities)[0],
                         size,
                         &(*eigenvalues)[0]);
#endif
}

// Compute inverse of A from scratch.
// Uses eigendecomposition of M'*M.
void
GenericFactor::Invert(const vector<double>& eigenvalues,
                      const vector<double>& eigenvectors)
{
#ifdef PRINT_INVERSION_STATS
    ++num_inversions_from_scratch;
    if (0 == num_inversions_from_scratch % 10000) {
        cout << "Number of incremental inversions: "
             << num_inversions_incremental << endl;
        cout << "Number of standard inversions: " << num_inversions_from_scratch
             << endl;
        cout << "Number of eigenvalue computations: "
             << num_eigenvalue_computations << endl;
    }
#endif
    int size = eigenvalues.size();
    int size_A = size + 1;
    inverse_A_.assign(size_A * size_A, 0.0);
    for (int i = 0; i < size; ++i) {
        double s = 1.0 / eigenvalues[i];
        const double* v = &eigenvectors[0] + i * size;
        for (int j = 0; j < size; ++j) {
            for (int k = j; k < size; ++k) {
                inverse_A_[(1 + j) * size_A + (1 + k)] += s * v[j] * v[k];
            }
        }
    }
    double s = 0.0;
    vector<double> d(size, 0.0);
    for (int j = 1; j <= size; ++j) {
        s -= inverse_A_[j * size_A + j];
        d[j - 1] += inverse_A_[j * size_A + j];
        for (int k = j + 1; k <= size; ++k) {
            inverse_A_[k * size_A + j] = inverse_A_[j * size_A + k];
            s -= 2 * inverse_A_[j * size_A + k];
            d[j - 1] += inverse_A_[j * size_A + k];
            d[k - 1] += inverse_A_[j * size_A + k];
        }
    }

    double invs = 1.0 / s;
    inverse_A_[0] = invs;
    for (int j = 1; j <= size; ++j) {
        inverse_A_[j * size_A] = -invs * d[j - 1];
        inverse_A_[j] = inverse_A_[j * size_A];
        inverse_A_[j * size_A + j] += invs * d[j - 1] * d[j - 1];
        for (int k = j + 1; k <= size; ++k) {
            inverse_A_[j * size_A + k] += invs * d[j - 1] * d[k - 1];
            inverse_A_[k * size_A + j] = inverse_A_[j * size_A + k];
        }
    }
}

// Checks if M'*M is singular.
// If so, asserts that the null space is 1-dimensional
// and returns a basis of the null space.
bool
GenericFactor::IsSingular(vector<double>& eigenvalues,
                          vector<double>& eigenvectors,
                          vector<double>* null_space_basis)
{
    int size = eigenvalues.size();
    int zero_eigenvalue = -1;
    for (int i = 0; i < size; ++i) {
        if (eigenvalues[i] < 1e-12) {
            if (zero_eigenvalue >= 0) {
                cout << eigenvalues[i] << " " << eigenvalues[zero_eigenvalue]
                     << endl;
                assert(false);
            }
            zero_eigenvalue = i;
        }
    }
    if (zero_eigenvalue < 0)
        return false;
    if (null_space_basis) {
        null_space_basis->assign(eigenvectors.begin() + zero_eigenvalue * size,
                                 eigenvectors.begin() +
                                   (1 + zero_eigenvalue) * size);
    }
    return true;
}

void
GenericFactor::SolveQP(const vector<double>& variable_log_potentials,
                       const vector<double>& additional_log_potentials,
                       vector<double>* variable_posteriors,
                       vector<double>* additional_posteriors)
{
    if (verbosity_ > 5)
        cout << "Solving QP..." << endl;
    size_t dim = variable_log_potentials.size();

    vector<double> variable_log_potentials_adj = variable_log_potentials;
    if (adjust_degrees_) {
        for (size_t j = 0; j < dim; ++j)
            variable_log_potentials_adj[j] /= sqrt_degrees_[j];
    }

    // Initialize the active set.
    if (active_set_.size() == 0) {
        variable_posteriors->resize(variable_log_potentials.size());
        additional_posteriors->resize(additional_log_potentials.size());
        distribution_.clear();

        // Initialize the active set with one vertex.
        // For best convergence, it's good to use the MAP.
        // However you may specify a custom initializer.
       Configuration configuration = init_configuration_;

        if (configuration == nullptr)
        {
            // Initialize by solving the LP, discarding the quadratic term.
            configuration = CreateConfiguration();

            double value;
            Maximize(variable_log_potentials_adj,
                     additional_log_potentials,
                     configuration,
                     &value);
        }

        active_set_.push_back(configuration);
        distribution_.push_back(1.0);

        // Initialize inv(A) as [-M,1;1,0].
        double nrm = CountCommonValuesAdapt(configuration, configuration);
        inverse_A_ = { -nrm, 1, 1, 0 };
    }
    if (verbosity_ > 5)
    {
        cout << "Distribution at beginning of active set: ";
        for (auto val : distribution_)
            cout << val << " ";
        cout << endl;
    }

    bool changed_active_set = true;
    vector<double> z;
    int num_max_iterations = num_max_iterations_QP_;
    double tau = 0;
    for (int iter = 0; iter < num_max_iterations; ++iter) {

        if (verbosity_ > 6)
            cout << "active set iter " << iter << endl;

        bool same_as_before = true;
        bool unbounded = false;
        if (changed_active_set) {
            // Recompute vector b.
            vector<double> b(active_set_.size() + 1, 0.0);
            b[0] = 1.0;
            for (size_t i = 0; i < active_set_.size(); ++i) {
                const Configuration& configuration = active_set_[i];
                double score;
                Evaluate(variable_log_potentials_adj,
                         additional_log_potentials,
                         configuration,
                         &score);
                b[i + 1] = score;
            }

            // Solve the system Az = b.
            z.resize(active_set_.size());
            size_t size_A = active_set_.size() + 1;
            for (size_t i = 0; i < active_set_.size(); ++i) {
                z[i] = 0.0;
                for (size_t j = 0; j < size_A; ++j) {
                    z[i] += inverse_A_[(i + 1) * size_A + j] * b[j];
                }
            }
            tau = 0.0;
            for (size_t j = 0; j < size_A; ++j) {
                tau += inverse_A_[j] * b[j];
            }

            same_as_before = false;
        }

        if (same_as_before) {
            // Compute the variable marginals from the full distribution
            // stored in z.
            ComputeMarginalsFromSparseDistribution(
              active_set_, z, variable_posteriors, additional_posteriors);
            if (adjust_degrees_) {
                for (size_t j = 0; j < dim; ++j)
                    (*variable_posteriors)[j] /= sqrt_degrees_[j];
            }

            // Get the most violated constraint
            // (by calling the black box that computes the MAP).
            vector<double> scores = variable_log_potentials_adj;
            for (size_t i = 0; i < scores.size(); ++i) {
                scores[i] -= (*variable_posteriors)[i];
            }
            Configuration configuration = CreateConfiguration();
            double value;
            Maximize(scores, additional_log_potentials, configuration, &value);

            double very_small_threshold = 1e-9;
            if (value <= tau + very_small_threshold) { // value <= tau.
                // We have found the solution;
                // the distribution, active set, and inv(A) are cached for the
                // next round.
                if (verbosity_ > 2) {
                    cout << "Converged." << endl;
                }
                DeleteConfiguration(configuration);
                return;
            } else {
                for (size_t k = 0; k < active_set_.size(); ++k) {
                    // This is expensive and should just be a sanity check.
                    // However, in practice, numerical issues force an already
                    // existing configuration to try to be added. Therefore, we
                    // always check if a configuration already exists before
                    // inserting it. If it does, that means the active set
                    // method converged to a solution (but numerical issues had
                    // prevented us to see it.)
                    if (SameConfiguration(active_set_[k], configuration)) {
                        if (verbosity_ > 0) {
                            cout << "Warning: value - tau = " << value - tau
                                 << " " << value << " " << tau << endl;
                        }
                        // We have found the solution;
                        // the distribution, active set, and inv(A)
                        // are cached for the next round.
                        DeleteConfiguration(configuration);

                        // Just in case, clean the cache.
                        // This may prevent eventual numerical problems in the
                        // future.
                        if (clear_cache_) {
                            for (size_t j = 0; j < active_set_.size(); ++j) {
                                if (j == k)
                                    continue; // This configuration was deleted
                                              // already.
                                DeleteConfiguration(active_set_[j]);
                            }
                            active_set_.clear();
                            inverse_A_.clear();
                            distribution_.clear();
                        }

                        // Return.
                        return;
                    }
                }
                z.push_back(0.0);
                distribution_ = z;

                // Update inv(A).
                bool singular =
                  !InvertAfterInsertion(active_set_, configuration);
                if (singular) {
                    // If adding a new configuration causes the matrix to be
                    // singular, don't just add it. Instead, look for a
                    // configuration in the null space and remove it before
                    // inserting the new one. Right now, if more than one such
                    // configuration exists, we just remove the first one we
                    // find. There's a chance this could cause some cyclic
                    // behaviour. If that is the case, we should randomize this
                    // choice. Note: This step is expensive and requires an
                    // eigendecomposition.
                    // TODO: I think there is a graph interpretation for this
                    // problem. Maybe some specialized graph algorithm is
                    // cheaper than doing the eigendecomposition.
                    vector<double> similarities(active_set_.size() *
                                                active_set_.size());
                    ComputeActiveSetSimilarities(active_set_, &similarities);
                    vector<double> padded_similarities(
                      (active_set_.size() + 2) * (active_set_.size() + 2), 1.0);
                    for (size_t i = 0; i < active_set_.size(); ++i) {
                        for (size_t j = 0; j < active_set_.size(); ++j) {
                            padded_similarities[(i + 1) *
                                                  (active_set_.size() + 2) +
                                                (j + 1)] =
                              similarities[i * active_set_.size() + j];
                        }
                    }
                    padded_similarities[0] = 0.0;
                    for (size_t i = 0; i < active_set_.size(); ++i) {
                        double value =
                          CountCommonValuesAdapt(configuration, active_set_[i]);
                        padded_similarities[(i + 1) * (active_set_.size() + 2) +
                                            (active_set_.size() + 1)] = value;
                        padded_similarities[(active_set_.size() + 1) *
                                              (active_set_.size() + 2) +
                                            (i + 1)] = value;
                    }
                    double value =
                      CountCommonValuesAdapt(configuration, configuration);
                    padded_similarities[(active_set_.size() + 1) *
                                          (active_set_.size() + 2) +
                                        (active_set_.size() + 1)] = value;

                    vector<double> eigenvalues(active_set_.size() + 2);
                    EigenDecompose(&padded_similarities, &eigenvalues);
                    int zero_eigenvalue = -1;
                    for (size_t i = 0; i < active_set_.size() + 2; ++i) {
                        if (NEARLY_EQ_TOL(eigenvalues[i], 0.0, 1e-9)) {
                            if (zero_eigenvalue >= 0) {
                                // If this happens, something failed. Maybe a
                                // numerical problem may cause this. In that
                                // case, just give up, clean the cache and
                                // return. Hopefully the next iteration will fix
                                // it.
                                cout << "Multiple zero eigenvalues: "
                                     << eigenvalues[zero_eigenvalue] << " and "
                                     << eigenvalues[i] << endl;
                                cout << "Warning: Giving up." << endl;
                                // Clean the cache.
                                for (size_t j = 0; j < active_set_.size(); ++j) {
                                    DeleteConfiguration(active_set_[j]);
                                }
                                active_set_.clear();
                                inverse_A_.clear();
                                distribution_.clear();
                                return;
                            }
                            zero_eigenvalue = i;
                        }
                    }
                    assert(zero_eigenvalue >= 0);
                    vector<int> configurations_to_remove;
                    for (size_t j = 1; j < active_set_.size() + 1; ++j) {
                        double value =
                          padded_similarities[zero_eigenvalue *
                                                (active_set_.size() + 2) +
                                              j];
                        if (!NEARLY_EQ_TOL(value, 0.0, 1e-9)) {
                            configurations_to_remove.push_back(j - 1);
                        }
                    }
                    if (verbosity_ > 2) {
                        cout << "Pick a configuration to remove ("
                             << configurations_to_remove.size() << " out of "
                             << active_set_.size() << ")." << endl;
                    }

                    assert(configurations_to_remove.size() >= 1);
                    int j = configurations_to_remove[0];

                    // Update inv(A).
                    InvertAfterRemoval(active_set_, j);

                    // Remove blocking constraint from the active set.
                    DeleteConfiguration(
                      active_set_[j]); // Delete configutation.
                    active_set_.erase(active_set_.begin() + j);

                    singular =
                      !InvertAfterInsertion(active_set_, configuration);
                    assert(!singular);
                }

                // Insert configuration to active set.
                if (verbosity_ > 2) {
                    cout << "Inserted one element to the active set (iteration "
                         << iter << ")." << endl;
                }
                active_set_.push_back(configuration);
                changed_active_set = true;
            }
        } else {
            // Solution has changed from the previous iteration.
            // Look for blocking constraints.
            int blocking = -1;
            bool exist_blocking = false;
            double alpha = 1.0;
            for (size_t i = 0; i < active_set_.size(); ++i) {

                // Incorrect factors can make this fail.
                assert(distribution_[i] >= -1e-12);

                if (z[i] >= distribution_[i])
                    continue;
                if (z[i] < 0)
                    exist_blocking = true;
                double tmp = distribution_[i] / (distribution_[i] - z[i]);
                if (blocking < 0 || tmp < alpha) {
                    alpha = tmp;
                    blocking = i;
                }
            }

            if (!exist_blocking) {
                // No blocking constraints.
                assert(!unbounded);
                distribution_ = z;
                alpha = 1.0;
                changed_active_set = false;
            } else {
                if (alpha > 1.0 && !unbounded)
                    alpha = 1.0;
                // Interpolate between factor_posteriors_[i] and z.
                if (alpha == 1.0) {
                    distribution_ = z;
                } else {
                    for (size_t i = 0; i < active_set_.size(); ++i) {
                        z[i] = (1 - alpha) * distribution_[i] + alpha * z[i];
                        distribution_[i] = z[i];
                        if (distribution_[i] < 0.0) {
                            if (verbosity_ > 2) {
                                cout << "Truncating distribution variable: "
                                     << distribution_[i] << endl;
                            }
                            distribution_[i] = 0.0;
                        }
                    }
                }

                // Update inv(A).
                InvertAfterRemoval(active_set_, blocking);

                // Remove blocking constraint from the active set.
                if (verbosity_ > 2) {
                    cout << "Removed one element to the active set (iteration "
                         << iter << ")." << endl;
                }

                DeleteConfiguration(
                  active_set_[blocking]); // Delete configutation.
                active_set_.erase(active_set_.begin() + blocking);

                z.erase(z.begin() + blocking);
                distribution_.erase(distribution_.begin() + blocking);
                changed_active_set = true;
                for (size_t i = 0; i < distribution_.size(); ++i) {
                    assert(distribution_[i] > -1e-16);
                }
            }
        }
    }

    if (verbosity_ > 2) {
        cout << "Maximum number of iterations reached." << endl;
    }

    // Maximum number of iterations reached.
    // Return the best existing solution by computing the variable marginals
    // from the full distribution stored in z.
    // assert(false);
    ComputeMarginalsFromSparseDistribution(
      active_set_, z, variable_posteriors, additional_posteriors);
    if (adjust_degrees_) {
        for (size_t j = 0; j < dim; ++j)
            (*variable_posteriors)[j] /= sqrt_degrees_[j];
    }
}

/* Get the correspondence between configurations & variable/additionals */
void
GenericFactor::GetCorrespondence(vector<double>* M, vector<double>* N)
{

    size_t n_active = active_set_.size();
    size_t n_vars = Degree();
    size_t n_add = GetAdditionalLogPotentials().size();

    M->reserve(n_vars * n_active);
    N->reserve(n_add * n_active);

    vector<double> m_row(n_vars), n_row(n_add);

    for (size_t i = 0; i < n_active; ++i) {

        m_row.assign(n_vars, 0);
        n_row.assign(n_add, 0);

        UpdateMarginalsFromConfiguration(active_set_[i], 1.0, &m_row, &n_row);
        if (adjust_degrees_)
            for (size_t j = 0; j < n_vars; ++j)
                m_row[j] /= sqrt_degrees_[j];

        M->insert(M->end(), m_row.begin(), m_row.end());

        N->insert(N->end(), n_row.begin(), n_row.end());
    }
}

// multiply by the lower-right corner of inverse_A_, which is our Q
void
GenericFactor::QVec(const double* v, double* out)
{
    size_t nnz = active_set_.size();
    for (size_t i = 0; i < nnz; ++i)
        for (size_t j = 0; j < nnz; ++j)
            out[i] += inverse_A_[(1 + nnz) * (i + 1) + (j + 1)] * v[j];
}

void
GenericFactor::JacobianVec(const vector<double>& v,
                           vector<double>& out,
                           vector<double>& out_add)
{
    size_t nnz = active_set_.size();
    out_add.assign(additional_log_potentials_.size(), 0.0);

    // 1. Multiply v by M.t, save in buf
    vector<double> Mtv(nnz); // TODO avoid the allocation
    vector<double> QMtv(nnz, 0);
    for (size_t i = 0; i < nnz; ++i) {
        Evaluate(v, out_add, active_set_[i], &Mtv[i]);
    }

    // 2. Apply jac-vector to buf
    QVec(Mtv.data(), QMtv.data());

    // 3. Multiply result by M, store in out and out_add.
    ComputeMarginalsFromSparseDistribution(active_set_, QMtv, &out, &out_add);
}

void
GenericFactor::DistJacobianVec(const vector<double>& v,
                               vector<double>& out,
                               vector<double>& out_add)
{
    // Gradient wrt the posterior probabilties on the support.
    size_t nnz = active_set_.size();
    out_add.assign(GetNumAdditionals(), 0.0);

    vector<double> Qv(nnz, 0);

    // 2. Apply jac-vector to input vector v
    QVec(v.data(), Qv.data());

    // 3. Multiply result by M, store in out and out_add.
    ComputeMarginalsFromSparseDistribution(active_set_, Qv, &out, &out_add);
}

} // namespace AD3


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_IO_MATRIX_MARKET_INCLUDE
#define MTL_IO_MATRIX_MARKET_INCLUDE

#include <string>
#include <fstream>
#include <iostream>
#include <limits>
#include <locale>
#include <complex>

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_integral.hpp>
// #include <boost/algorithm/string/case_conv.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/numeric/mtl/io/matrix_file.hpp>
#include <boost/numeric/mtl/io/read_filter.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/string_to_enum.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/operation/set_to_zero.hpp>

namespace mtl { namespace io {

/// Input file stream for files in matrix market format
class matrix_market_istream
{
    class pattern_type {};
    typedef matrix_market_istream        self;
    void check_stream(const std::string& file_name= std::string()) // not const to delete new_stream
    {
	if (!my_stream.good()) {
	    std::string message("matrix_market_istream: Error in input stream!\n");
	    if (file_name != std::string())
		message+= "Probably file " + file_name + " not found.\n";
	    std::cerr << message;
	    if (new_stream)
		delete new_stream, new_stream= 0; // To get valgrind quite
	    throw(file_not_found(message.c_str()));
	}
    }

  public:
    explicit matrix_market_istream(const char* p) : new_stream(new std::ifstream(p)), my_stream(*new_stream) { check_stream(p); }
    explicit matrix_market_istream(const std::string& s) : new_stream(new std::ifstream(s.c_str())), my_stream(*new_stream) { check_stream(s); }
    explicit matrix_market_istream(std::istream& s= std::cin) : new_stream(0), my_stream(s) { check_stream(); }

    ~matrix_market_istream()
    { 	if (new_stream) delete new_stream;    }

    template <typename Coll>
    self& operator>>(Coll& c)
    { 	return read(c, typename mtl::traits::category<Coll>::type());    }

    /// Close only my own file, i.e. if filename and not stream is passed in constructor
    void close() { if (new_stream) new_stream->close(); }

  protected:
    template <typename Matrix> self& read(Matrix& A, tag::matrix);

    void to_lower(std::string& s) const
    {
	using namespace std;
	for (unsigned i= 0; i < s.size(); i++)
	    s[i]= tolower(s[i]);
    }

    void set_symmetry(std::string& symmetry_text)
    {
	to_lower(symmetry_text);
	const char* symmetry_options[]= {"general", "symmetric", "skew-symmetric", "hermitian"};
	my_symmetry= string_to_enum(symmetry_text, symmetry_options, symmetry());
    }

    void set_sparsity(std::string& sparsity_text)
    {
	to_lower(sparsity_text);
	const char* sparsity_options[]= {"coordinate", "array"};
	my_sparsity= string_to_enum(sparsity_text, sparsity_options, sparsity());
    }

    template <typename Inserter, typename Value>
    void read_matrix(Inserter& ins, Value)
    {
	typedef typename Collection<typename Inserter::matrix_type>::size_type size_type;
	read_filter<Inserter> filter(ins);
	if (my_sparsity == coordinate) // sparse
	    // while (my_stream) { // sometimes does an extra erroneous loop
	    for (std::size_t i= 0; i < nnz; i++) {
		size_type r, c;
		my_stream >> r >> c;
		if (!my_stream) break; // in case while(my_stream) caught an empty line at the end
		insert_value(ins, r-1, c-1, filter, Value());
	    }
	else // dense
	    for (std::size_t c= 0; c < ncols; c++)
		for (std::size_t r= 0; r < nrows; r++)
		    insert_value(ins, r, c, filter, Value());
    }

    template <typename Inserter, typename Filter, typename Value>
    void insert_value(Inserter& ins, std::size_t r, std::size_t c, const Filter& filter, Value)
    {
	using mtl::conj;
	typedef typename Collection<typename Inserter::matrix_type>::value_type mvt;
	Value v;
	read_value(v);
	// std::cout << "Going to insert at [" << r << "][" << c << "] value " << which_value(v, mvt()) << "\n";
	if (filter(r, c))
	    ins[r][c] << which_value(v, mvt());
	if (r != c && filter(c, r))
	    switch(my_symmetry) {
	      case symmetric:      ins[c][r] << which_value(v, mvt()); break;
	      case skew:           ins[c][r] << -which_value(v, mvt()); break;
	      case Hermitian:      ins[c][r] << conj(which_value(v, mvt())); break;
	      default:             ; // do nothing
	    }
    }

    void read_value(pattern_type) {}
    void read_value(double& v) { my_stream >> v;}
    void read_value(long& v) { my_stream >> v;}
    void read_value(std::complex<double>& v)
    {
	double r, i; my_stream >> r >> i; v= std::complex<double>(r, i);
    }

    // Which value to be inserted? Itself if exist and 0 for pattern; complex are
    template <typename Value, typename MValue> MValue which_value(Value v, MValue) { return boost::numeric_cast<MValue>(v); }
    template <typename MValue> MValue which_value(pattern_type, MValue) { return boost::numeric_cast<MValue>(0.0); }
    template <typename MValue> MValue which_value(std::complex<double>, MValue) { MTL_THROW(runtime_error("Cannot convert complex value in real\n")); return 1; }
    std::complex<long double> which_value(std::complex<double> v, std::complex<long double>) { return boost::numeric_cast<std::complex<long double> >(v); }
    std::complex<double> which_value(std::complex<double> v, std::complex<double>) { return v; }
    std::complex<float> which_value(std::complex<double> v, std::complex<float>) { return std::complex<float>(float(real(v)), float(imag(v))); }

    std::ifstream      *new_stream;
    std::istream       &my_stream;
    enum symmetry {general, symmetric, skew, Hermitian} my_symmetry;
    enum sparsity {coordinate, array} my_sparsity;
    std::size_t nrows, ncols, nnz;
};

// Matrix version
template <typename Matrix>
matrix_market_istream& matrix_market_istream::read(Matrix& A, tag::matrix)
{
    std::string marker, type, sparsity_text, value_format, symmetry_text;
    my_stream >> marker >> type >> sparsity_text >> value_format >> symmetry_text;
#if 0
    std::cout << marker << ", " << type << ", " << sparsity_text << ", "
	      << value_format << ", " << symmetry_text << "\n";
#endif
    MTL_THROW_IF(marker != std::string("%%MatrixMarket"),
		 runtime_error("File not in Matrix Market format"));
    MTL_THROW_IF(type != std::string("matrix"),
		 runtime_error("Try to read matrix from non-matrix file"));

    set_symmetry(symmetry_text);
    set_sparsity(sparsity_text);

    char first, comment[80];
    do {
	my_stream >> first;
	if (first == '%') // comments start with % -> ignore them
	    my_stream.getline(comment, 80, '\n'); // read rest of line
	else
	    my_stream.putback(first); // if not commment we still need it
    } while (first == '%');

    my_stream >> nrows >> ncols;
    // std::cout << nrows << "x" << ncols << ", " << nnz << " non-zeros\n";
    A.change_dim(nrows, ncols);
    set_to_zero(A);

    std::size_t slot_size;
    if (sparsity_text == std::string("coordinate")) {
	my_stream >> nnz; slot_size= std::max(std::size_t(double(nnz) / double(A.dim1()) * 1.25), std::size_t(1));
    } else
	slot_size= A.dim2(); // maximal value (if A is dense it does not matter anyway)

    // Create enough space in sparse matrices
    mat::inserter<Matrix> ins(A, slot_size);

    if (value_format == std::string("real"))
	read_matrix(ins, double());
    else if (value_format == std::string("integer"))
	read_matrix(ins, long());
    else if (value_format == std::string("complex"))
	read_matrix(ins, std::complex<double>());
    else if (value_format == std::string("pattern"))
	read_matrix(ins, pattern_type());
    else
	MTL_THROW(runtime_error("Unknown tag for matrix value type in file"));

    return *this;
}

class matrix_market_ostream
{
    typedef matrix_market_ostream        self;
public:
    explicit matrix_market_ostream(const char* p) : new_stream(new std::ofstream(p)), my_stream(*new_stream) {}
    explicit matrix_market_ostream(const std::string& s) : new_stream(new std::ofstream(s.c_str())), my_stream(*new_stream) {}
    explicit matrix_market_ostream(std::ostream& s= std::cout) : new_stream(0), my_stream(s) {}

    ~matrix_market_ostream() { if (new_stream) delete new_stream; }

    template <typename Coll>
    self& operator<<(const Coll& c)
    {
	return write(c, typename mtl::traits::category<Coll>::type());
    }

    /// Close only my own file, i.e. if filename and not stream is passed in constructor
    void close() { if (new_stream) new_stream->close(); }

private:
    template <typename Matrix> self& write(const Matrix& A, tag::matrix)
    {
	matrix_status_line(A);
	if (sparsity(A) == std::string("coordinate "))
	    return write_sparse_matrix(A);
	else
	    return write_dense_matrix(A);
    }

    template <typename Matrix> self& write_sparse_matrix(const Matrix& A)
    {
	my_stream << num_rows(A) << " " << num_cols(A) << " " << A.nnz() << "\n";

	typename mtl::traits::row<Matrix>::type             row(A);
	typename mtl::traits::col<Matrix>::type             col(A);
	typename mtl::traits::const_value<Matrix>::type     value(A);
	typedef typename mtl::traits::range_generator<tag::major, Matrix>::type  cursor_type;
	typedef typename mtl::traits::range_generator<tag::nz, cursor_type>::type icursor_type;

	for (cursor_type cursor = begin<tag::major>(A), cend = end<tag::major>(A); cursor != cend; ++cursor)
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		my_stream << row(*icursor)+1 << " " << col(*icursor)+1 << " ", write_value(value(*icursor)), my_stream << "\n";
	return *this;
    }

    template <typename Matrix> self& write_dense_matrix(const Matrix& A)
    {
	my_stream << num_rows(A) << " " << num_cols(A) << "\n";
	for (std::size_t c = 0; c < num_cols(A); ++c)
	    for (std::size_t r = 0; r < num_rows(A); ++r) {
		write_value(A[r][c]), my_stream << " ";
	    my_stream << "\n";
	}
	return *this;
    }

    template <typename Value>
    typename boost::enable_if<boost::is_integral<Value> >::type
    write_value(const Value& v) { my_stream << v; }

    template <typename Value>
    typename boost::enable_if<boost::is_floating_point<Value> >::type
    write_value(const Value& v)
    {
	my_stream.precision(std::numeric_limits<Value>::digits10 + 1);
	my_stream.setf(std::ios::scientific);
	my_stream << v;
	my_stream.unsetf(std::ios::scientific);
    }

    template <typename Value>
    void write_value(const std::complex<Value>& v)
    {
	my_stream.precision(std::numeric_limits<Value>::digits10 + 1);
	my_stream.setf(std::ios::scientific);
	my_stream << real(v) << " " << imag(v);
	my_stream.unsetf(std::ios::scientific);
    }

    // Will be generalized via traits::is_symmetric and alike
    template <typename Matrix>
    std::string symmetry(const Matrix&) const { return std::string("general\n"); }

    template <typename Matrix>
    std::string sparsity(const Matrix&) const
    {
	return std::string( mtl::traits::is_sparse<Matrix>::value ? "coordinate " : "array " );
    }

    template <typename Value>
    typename boost::enable_if<boost::is_integral<Value>, std::string>::type
    value(const Value&) const { return std::string("integer "); }

    template <typename Value>
    typename boost::enable_if<boost::is_floating_point<Value>, std::string>::type
    value(const Value&) const { return std::string("real "); }

    template <typename Value>
    std::string value(const std::complex<Value>&) const { return std::string("complex "); }

    template <typename Matrix>
    void matrix_status_line(const Matrix& A) const
    {
	typedef typename Collection<Matrix>::value_type value_type;
	std::string st(std::string("%%MatrixMarket  matrix ") + sparsity(A) + value(value_type()) + symmetry(A));
	my_stream << st;
    }

protected:
    std::ofstream      *new_stream;
    std::ostream       &my_stream;
};

}} // namespace mtl::io

#endif // MTL_IO_MATRIX_MARKET_INCLUDE