#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 "../Self.h"

#include <vector>

namespace meow {

/*!
 * @brief Harris corner detect
 *
 * @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;
public:
  typedef FeaturePoint<double, double> MyFeaturePoint;
  typedef std::vector<MyFeaturePoint>  MyFeaturePoints;
  //! @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();
  }
  
  /*! @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 I for each pixel
    Bitmap<double> Ixx(input.height(), input.width(), 0.0);
    Bitmap<double> Iyy(input.height(), input.width(), 0.0);
    Bitmap<double> Ixy(input.height(), input.width(), 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));
        Ixx.pixel(y, x, gx * gx);
        Iyy.pixel(y, x, gy * gy);
        Ixy.pixel(y, x, gx * gy);
      }
    }
    
    // blur
    Ixx.gaussianed(self->sizeW_, self->sizeW_);
    Iyy.gaussianed(self->sizeW_, self->sizeW_);
    Ixy.gaussianed(self->sizeW_, self->sizeW_);
    
    // filter too flat or on edge
    Bitmap<double>  R(input.height(), input.width(),   0.0);
    Bitmap<bool> good(input.height(), input.width(), false);
    ssize_t b = self->boundB_;
    for (ssize_t y = b, Y = -b + input.height(); y < Y; y++) {
      for (ssize_t x = b, X = -b + input.width(); x < X; x++) {
        double det = Ixx(y, x) * Iyy(y, x) - squ(Ixy(y, x));
        double tra = Ixx(y, x) + Iyy(y, x);
        double r = det - self->ratioK_ * squ(tra);
        R.pixel(y, x, r);
        good.pixel(y, x, (r >= self->thresholdR_));
      }
    }
    
    // find union neighbor
    DisjointSet dsj(input.size());
    ssize_t dy[2] = {0, 1};
    ssize_t dx[2] = {1, 0};
    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)){
          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_);

    MyFeaturePoints ret;
    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};
        std::vector<double> desc; // description
        for (ssize_t d = 1; d <= (ssize_t)self->boundB_; d++) {
          std::vector<double> light;
          size_t max_id = 0;
          size_t x0 = x - d, y0 = y - d;
          for (size_t k = 0; k < 4; k++) {
            for (ssize_t n = 0;
                 n < (ssize_t)b * 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[-1 + light.size()]) {
                max_id = -1 + (ssize_t)light.size();
              }
            }
          }
          for (ssize_t n = 0, N = light.size(); n < N; n++) {
            desc.push_back((max_id + n) % N);
            desc.push_back(light[(max_id + n) % N]);
          }
        }
        MyFeaturePoint now(2, desc.size());
        now.position(0, x);
        now.position(1, y);
        now.description(Vector<double>(desc));
        ret.push_back(now);
      }
    }
    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
};

}

#endif // gra_FeaturePointsDetector_Harris