#ifndef   gra_FeaturePointsMatch_K_Match_H__
#define   gra_FeaturePointsMatch_K_Match_H__

#include "FeaturePointsMatch.h"

#include "../Self.h"
#include "../dsa/VP_Tree.h"
#include "../oo/ObjBase.h"

#include <cstdlib>

namespace meow {

template<class Scalar, class Description>
class FeaturePointsMatch_K_Match:
public FeaturePointsMatch<Scalar, Description> {
# define FPMKM FeaturePointsMatch_K_Match
public:
  typedef std::vector<FeaturePoint<Scalar, Description> > FeaturePoints ;
  typedef std::vector<FeaturePoints                     > FeaturePointss;
private:
  struct Node {
    size_t                   id_;
    size_t                index_;
    FeaturePointss const*   ptr_;

    Node() {
    }
    Node(Node const& nd) {
      id_    = nd.   id_;
      index_ = nd.index_;
      ptr_   = nd.  ptr_;
    }
    Node(size_t id, size_t index, FeaturePointss const* ptr) {
      id_    = id;
      index_ = index;
      ptr_   = ptr;
    }
    ~Node() {
    }
    bool operator<(Node const& nd) const {
      return (id_ < nd.id_);
    }
    Description operator[](size_t id) const {
      return (*ptr_)[id_][index_][id];
    }
  };
  
  struct Myself {
    size_t k_;
    Myself() {
      k_ = 1;
    }
    Myself(size_t k): k_(k) {
    }
    Myself(Myself const& m): k_(m.k_) {
    }
    ~Myself() {
    }
  };

  Self<Myself> const self;
public:
  FPMKM(): self() {
  }

  FPMKM(FPMKM const& m): self(m.self, Self<Myself>::COPY_FROM) {
    self().copyFrom(m.self);
  }

  FPMKM(size_t k): self(Myself(k)) {
  }

  ~FPMKM() {
  }

  FPMKM& copyFrom(FPMKM const& m) {
    self().copyFrom(m.self);
    return *this;
  }

  FPMKM& referenceFrom(FPMKM const& m) {
    self().referenceFrom(m.self);
    return *this;
  }

  size_t paramK() const {
    return self->k_;
  }

  size_t paramK(size_t k) {
    self()->k_ = std::max(k, (size_t)1);
    return paramK();
  }


  FeaturePointIndexPairs match(size_t dimension,
                               FeaturePoints const& from,
                               FeaturePoints const& to) const {
    return match(dimension, FeaturePointss(1, from), FeaturePointss(1, to));
  }


  FeaturePointIndexPairs match(size_t dimension,
                               FeaturePoints const& from,
                               FeaturePointss const& to) const {
    return match(dimension, FeaturePointss(1, from), to);
  }

  FeaturePointIndexPairs match(size_t dimension,
                               FeaturePointss const& from,
                               FeaturePointss const& to) const {
    VP_Tree<Node, Description> tree(dimension);
    for (size_t i = 0, I = to.size(); i < I; i++) {
      for (size_t j = 0, J = to[i].size(); j < J; j++) {
        tree.insert(Node(i, j, &to));
      }
    }
    FeaturePointIndexPairs ret(from.size());
    for (size_t i = 0, I = from.size(); i < I; i++) {
      for (size_t j = 0, J = from[i].size(); j < J; j++) {
        Node now(i, j, &from);
        std::vector<Node> tree_ret = tree.query(now, self->k_, true);
        for (size_t k = 0, K = tree_ret.size(); k < K; k++) {
          ret.push_back(FeaturePointIndexPair(i, j,
                                              tree_ret[k].id_,
                                              tree_ret[k].index_));
        }
      }
    }
    return ret;
  }

  FeaturePointIndexPairs match(size_t dimension,
                               FeaturePointss const& fpss) const {
    FeaturePointIndexPairs ret(fpss.size()), add;
    FeaturePointss to(fpss);
    for (size_t i = 0, I = fpss.size(); i < I; i++) {
      FeaturePoints tmp(to[i]);
      to[i].clear();
      add = match(dimension, fpss[i], to);
      for (size_t j = 0, J = add.size(); j < J; j++) {
        ret.push_back(FeaturePointIndexPair(i              , add[j].from.second,
                                            add[j].to.first, add[j].to.second));
      }
      to[i] = tmp;
    }
    return ret;
  }
  
  FPMKM& operator=(FPMKM const& b) {
    return copyFrom(b);
  }


  bool write(FILE* f, bool bin, unsigned int fg) const {
    // TODO
    return false;
  }

  bool read (FILE* f, bool bin, unsigned int fg) {
    // TODO
    return false;
  }

  ObjBase* create() const {
    return new FPMKM();
  }

  ObjBase* copyFrom(ObjBase const* ptr) {
    return &(copyFrom(*(FPMKM*)ptr));
  }

  char const* ctype() const {
    return typeid(*this).name();
  }

  std::string type() const {
    return std::string(ctype());
  }
# undef FPMKM
};

} // meow

#endif // gra_FeaturePointsMatch_K_Match_H__