#ifndef   gra_FeaturePointsDetector_Harris
#define   gra_FeaturePointsDetector_Harris

#include "FeaturePointsDetector.h"

#include "Bitmap.h"
#include "FeaturePoint.h"
#include "FeaturePointsDetector.h"

#include "../dsa/DisjointSet.h"
#include "../math/utility.h"

#include "../Self.h"

#include <vector>
#include <algorithm>

namespace meow {

/*!
 * @brief Harris-Corner-Detect algorithm for finding feature points.
 *
 * @author cat_leopard
 */
template<class Pixel>
class FeaturePointsDetector_Harris: public FeaturePointsDetector<Pixel> {
# define FPD_Harris FeaturePointsDetector_Harris
private:
  struct Myself {
    double     ratioK_;
    double thresholdR_;
    double      sizeW_;
    double     noiseN_;
    double     lightL_;
    double   featureG_;
    size_t     boundB_;

    Myself():
    ratioK_(0.03),
    thresholdR_(0.001),
    sizeW_(2.0),
    noiseN_(3.0),
    lightL_(30.0),
    featureG_(3.0),
    boundB_(10u) {
    }
    Myself(Myself const& m):
    ratioK_(m.ratioK_),
    thresholdR_(m.thresholdR_),
    sizeW_(m.sizeW_),
    noiseN_(m.noiseN_),
    lightL_(m.lightL_),
    featureG_(m.featureG_),
    boundB_(m.boundB_){
    }
    ~Myself() {
    }
  };

  Self<Myself> const self;

  typedef FeaturePoint<double, double> MyFeaturePoint;
  typedef std::vector<MyFeaturePoint>  MyFeaturePoints;
public:
  //! @brief constructor 使用預設參數
  FPD_Harris(): self() {
  }

  //! @brief constructor 參數複製自另一個 FeaturePointsDetector_Harris
  FPD_Harris(FPD_Harris const& fps): self(fps.self, Self<Myself>::COPY_FROM) {
  }

  //! @brief 解構子
  ~FPD_Harris() {
  }

  //! @brief 複製
  FPD_Harris& copyFrom(FPD_Harris const& fps) {
    self().copyFrom(fps.self);
    return *this;
  }

  //! @brief 參照
  FPD_Harris& referenceFrom(FPD_Harris const& fps) {
    self().referenceFrom(fps.self);
    return *this;
  }

  //! @brief K
  double paramK() const {
    return self->ratioK_;
  }

  //! @brief R
  double paramR() const {
    return self->thresholdR_;
  }

  //! @brief W
  double paramW() const {
    return self->sizeW_;
  }

  //! @brief N
  double paramN() const {
    return self->noiseN_;
  }

  //! @brief G
  double paramG() const {
    return self->featureG_;
  }

  //! @brief L
  double paramL() const {
    return self->lightL_;
  }

  //! @brief bound
  size_t paramB() const {
    return self->boundB_;
  }

  //! @brief K
  double paramK(double k) {
    self()->ratioK_ = k;
    return paramK();
  }

  //! @brief R
  double paramR(double r) {
    self()->thresholdR_ = r;
    return paramR();
  }

  //! @brief W
  double paramW(double w) {
    self()->sizeW_ = w;
    return paramW();
  }

  //! @brief N
  double paramN(double n){
    self()->noiseN_ = n;
    return paramN();
  }

  //! @brief L
  double paramL(double l) {
    self()->lightL_ = l;
    return paramL();
  }

  //! @brief G
  double paramG(double g) {
    self()->featureG_ = g;
    return paramG();
  }

  //! @brief B
  size_t paramB(size_t b) {
    self()->boundB_ = b;
    return paramB();
  }

  size_t descriptionDimension() const {
    return (squ(self->boundB_ * 2 + 1) - 1) * 2;
  }

  /*! @brief 找出特徵點
   *
   *  @param [in] bmp 要抓特徵點的點陣圖
   *  @return \c std::vector<FeaturePoint<double,double>> 型態的一堆特徵點
   */
  MyFeaturePoints detect(Bitmap<Pixel> const& bmp) const {
    Bitmap<Pixel> input = bmp;

    // gradiance
    Bitmap<Pixel> input_gx(input.gradianceX(0, self->noiseN_));
    Bitmap<Pixel> input_gy(input.gradianceY(self->noiseN_, 0));

    // get Matrix Ixx, Iyy, Ixy for each pixel
    Bitmap<Vector3D<double> > Ixys(input.height(), input.width(),
                                   Vector3D<double>(0.0));
    for (ssize_t y = 0, Y = input.height(); y < Y; y++)
      for (ssize_t x = 0, X = input.width(); x < X; x++) {
        Pixel gx(input_gx(y, x));
        Pixel gy(input_gy(y, x));
        Ixys.pixel(y, x, Vector3D<double>(gx * gx, gy * gy, gx * gy));
      }

    // blur for window size
    Ixys.gaussianed(self->sizeW_, self->sizeW_);
    input_gx.clear();
    input_gy.clear();

    // filter too flat or on edge
    Bitmap<double>    R(input.height(), input.width(),   0.0);
    Bitmap<bool>   good(input.height(), input.width(), false);
    for (ssize_t y = 0, Y = input.height(); y < Y; y++)
      for (ssize_t x = 0, X = input.width(); x < X; x++) {
        double det = Ixys(y, x)(0) * Ixys(y, x)(1) - squ(Ixys(y, x)(2));
        double tra = Ixys(y, x)(0) + Ixys(y, x)(1);
        double r = det - self->ratioK_ * squ(tra);
        R.pixel(y, x, r);
        good.pixel(y, x, (r >= self->thresholdR_));
      }
    Ixys.clear();

    // find union neighbor
    DisjointSet dsj(input.size());
    ssize_t dy[2] = {0, 1};
    ssize_t dx[2] = {1, 0};
    for (ssize_t y = 0, Y = input.height(); y + 1 < Y; y++)
      for (ssize_t x = 0, X = input.width(); x + 1 < X; x++)
        if(good.pixel((size_t)y, (size_t)x))
          for (size_t k = 0; k < 2u; k++)
            if (good.pixel((size_t)(y + dy[k]), (size_t)(x + dx[k])))
              dsj.merge( y          * input.width() +  x,
                        (y + dy[k]) * input.width() + (x + dx[k]));

    // find local maximum
    std::vector<size_t> max_i(input.size());
    for (size_t i = 0, I = input.size(); i < I; i++) {
      max_i[i] = i;
    }
    for (size_t i = 0, I = input.size(); i < I; i++) {
      size_t ri = dsj.root(i);
      if (R.pixel(       i  / input.width(),        i  % input.width()) >
          R.pixel(max_i[ri] / input.width(), max_i[ri] % input.width())) {
        max_i[ri] = i;
      }
    }

    // blur before get description
    input.gaussianed(self->featureG_, self->featureG_);

    // Ignore side
    ssize_t b = std::max<int>(std::max<int>(self->boundB_,
                                            2 * self->sizeW_),
                                            2 * self->noiseN_);
    MyFeaturePoints ret;
    Vector<double> desc(descriptionDimension(), 0.0); // description
    for (ssize_t y = b, Y = -b + input.height(); y < Y; y++)
      for (ssize_t x = b, X = -b + input.width(); x < X; x++) {
        if (!good.pixel((size_t)y, (size_t)x)) continue;
        size_t i = y * input.width() + x;
        if (max_i[dsj.root(i)] != i) continue;
        ssize_t dx[4] = {1, 0, -1, 0};
        ssize_t dy[4] = {0, 1, 0, -1};
        size_t ct = 0;
        for (ssize_t d = 1; d <= (ssize_t)self->boundB_; ++d) {
          std::vector<double> light;
          size_t max_id = 0, x0 = x - d, y0 = y - d;
          for (size_t k = 0; k < 4; k++)
            for (ssize_t n = 0; n < (ssize_t)d * 2; n++,
                 x0 += dx[k], y0 += dy[k]) {
              Pixel diff = input.pixel(y0, x0) - input.pixel(y, x) * 0.2;
              light.push_back(diff * diff * self->lightL_);
              if (light[max_id] < light[(ssize_t)light.size() - 1])
                max_id = (ssize_t)light.size() - 1;
            }
          for (ssize_t n = 0, N = light.size(); n < N; n++) {
            desc.scalar(ct++,       (max_id + n) % N );
            desc.scalar(ct++, light[(max_id + n) % N]);
          }
        }
        ret.push_back(MyFeaturePoint(Vector2D<double>(x, y).matrix(), desc));
      }
    return ret;
  }

  //! @brief same as \c copyFrom(fps)
  FPD_Harris& operator=(FPD_Harris const& fps) {
    return copyFrom(fps);
  }

  //! @brief same as \c detect(bmp)
  MyFeaturePoints operator()(Bitmap<Pixel> const& bmp) const {
    return detect(bmp);
  }

  /*! @brief 寫到檔案裡
   *
   * 未完成
   */
  bool write(FILE* f, bool bin, unsigned int fg) const {
    // TODO
    return false;
  }

  /*! @brief 將資料讀入
   *
   * 未完成
   */
  bool read (FILE* f, bool bin, unsigned int fg) {
    // TODO
    return false;
  }

  /*! @brief new一個自己
   *
   *  @return 一個new出來的FeaturePointsDetector_Harris<Pixel>
   */
  ObjBase* create() const {
    return (ObjBase*)new FPD_Harris<Pixel>();
  }

  /*! @brief 複製資料
   *
   * 輸入型別是 \c ObjBase \c const*
   * 這裡假設實體其實是 \c FeaturePointsDetector_Harris.
   * 事實上這個method就只是幫忙轉型然後呼叫原本的\c copyFrom
   *
   *  @param [in] b 資料來源
   *  @return this
   */
  ObjBase* copyFrom(ObjBase const* b) {
    return &(copyFrom(*(FPD_Harris const*)b));
  }

  /*! @brief 回傳class的type
   *
   *  @return \c char \c const\c * 形式的typename
   */
  char const* ctype() const {
    return typeid(*this).name();
  }

  /*! @brief 回傳class的type
   *
   *  @return \c std::string 形式的typename
   */
  std::string  type() const {
    return std::string(ctype());
  }
# undef FPD_Harris
};

} // meow

#endif // gra_FeaturePointsDetector_Harris