// This is brl/bseg/sdet/sdet_harris_detector.cxx
#include "sdet_harris_detector.h"
//:
// \file
#include <vcl_cstdlib.h>   // for vcl_abs(int) and vcl_qsort()
#include <vil1/vil1_memory_image_of.h>
#include <vil/vil_image_view.h>
#include <vil/vil_convert.h>
#include <vil/algo/vil_corners.h>
#include <brip/brip_vil1_float_ops.h>
#include <brip/brip_vil_float_ops.h>
#include <vsol/vsol_point_2d.h>

//: A container to support sorting of corners
//  Will result in descending order according to strength
struct sdet_harris_point
{
  sdet_harris_point () {}

  void set_point(vsol_point_2d_sptr const& p) {p_ = p;}
  void set_strength(const float s) {strength_ = s;}
  vsol_point_2d_sptr point() {return p_;}
  double strength() {return strength_;}

 private:
  float strength_;
  vsol_point_2d_sptr p_;
};

//The sort compare function
static int compare(sdet_harris_point*  pa,
                   sdet_harris_point*  pb)
{
  if (pa->strength() < pb->strength())
    return +1;
  return -1;
}

//---------------------------------------------------------------
// Constructors
//
//----------------------------------------------------------------

//: constructor from a parameter block (the only way)
//
sdet_harris_detector::sdet_harris_detector(sdet_harris_detector_params& rpp)
  : sdet_harris_detector_params(rpp)
{
  image_ = 0;
  vimage_ = 0;
  //don't really know but have to pick one
  use_vil_image_ = true;
}

//:Default Destructor
sdet_harris_detector::~sdet_harris_detector()
{
}

//-------------------------------------------------------------------------
//: Set the image to be processed
//
void sdet_harris_detector::set_image(vil1_image const& image)
{
  if (!image)
  {
    vcl_cout <<"In sdet_harris_detector::set_image(.) - null input\n";
    return;
  }
  points_valid_ = false;
  image_ = image;
  use_vil_image_ = false;
}

//-------------------------------------------------------------------------
//: Set the image resource to be processed
//
void sdet_harris_detector::set_image_resource(vil_image_resource_sptr const& image)
{
  if (!image)
  {
    vcl_cout <<"In sdet_harris_detector::set_image(.) - null input\n";
    return;
  }
  points_valid_ = false;
  vimage_ = image;
}

//------------------------------------------------------------------------
// : extract corners using vil1 code
//
bool sdet_harris_detector::extract_corners_vil1(vcl_vector<float>& x_pos,
                                                vcl_vector<float>& y_pos,
                                                vcl_vector<float>& val)
{
// Check the image
  if (!image_)
  {
    vcl_cout << "In sdet_harris_detector::extract_corners() - no image\n";
    return false;
  }
  int w = image_.width(), h = image_.height();
  vcl_cout << "sdet_harris_detector::extract_corners(): width = "
           << w << " height = " << h << vcl_endl;

  vil1_memory_image_of<float> inputf = brip_vil1_float_ops::convert_to_float(image_);
  vil1_memory_image_of<float> smooth = brip_vil1_float_ops::gaussian(inputf, sigma_);
  vil1_memory_image_of<float> IxIx, IxIy, IyIy, c;
  IxIx.resize(w,h);  IxIy.resize(w,h);   IyIy.resize(w,h);
  brip_vil1_float_ops::grad_matrix_NxN(smooth, n_, IxIx, IxIy, IyIy);
  c = brip_vil1_float_ops::harris(IxIx, IxIy, IyIy, scale_factor_);
  brip_vil1_float_ops::non_maximum_suppression(c, n_, thresh_, x_pos, y_pos, val);
  return true;
}

//------------------------------------------------------------------------
// : extract corners using vil code
//
bool sdet_harris_detector::extract_corners_vil(vcl_vector<float>& x_pos,
                                               vcl_vector<float>& y_pos,
                                               vcl_vector<float>& val)
{
// Check the image
  if (!vimage_||vimage_->nplanes()!=1)
  {
    vcl_cout << "In sdet_harris_detector::extract_corners() - "
             << "no image or not exactly one component\n";
    return false;
  }

  int w = vimage_->ni(), h = vimage_->nj();
  vcl_cout << "sdet_harris_detector::extract_corners(): width = "
           << w << " height = " << h << vcl_endl;

  vil_image_view<float> base_view = vimage_->get_view();

  vil_image_view<float> inputf;
  vil_convert_cast(base_view, inputf);
  vil_image_view<float> smooth = brip_vil_float_ops::gaussian(inputf, sigma_);
  vil_image_view<float> c(w,h);
  if (use_vil_harris_)
    vil_corners(smooth,c,scale_factor_);
  else
  {
    vil_image_view<float> IxIx(w,h), IxIy(w,h), IyIy(w,h);
    brip_vil_float_ops::grad_matrix_NxN(smooth, n_, IxIx, IxIy, IyIy);
    c = brip_vil_float_ops::harris(IxIx, IxIy, IyIy, scale_factor_);
  }
  brip_vil_float_ops::non_maximum_suppression(c, n_, thresh_, x_pos, y_pos, val);
  return true;
}

//--------------------------------------------------------------------------
//: extract a set of vsol_point_2d(s)
void sdet_harris_detector::extract_corners()
{
  if (points_valid_)
    return;

  //Process the image to extract the Harris corners
  points_.clear();
  vcl_vector<float> x_pos, y_pos, val;
  if (!use_vil_image_)
  { if (!extract_corners_vil1(x_pos, y_pos, val)) return; }
  else
  { if (!extract_corners_vil(x_pos, y_pos, val)) return; }
  int n_corners = x_pos.size();
  vcl_cout << "Found " << n_corners << " above the threshold\n";
  if (!n_corners)
  {
    vcl_cout << "sdet_harris_detector::extract_corners() - "
             << "no corners found\n";
    return;
  }
  //Sort the corners according to strength
  sdet_harris_point* point_array = new sdet_harris_point[n_corners];
  for (int i = 0; i<n_corners; i++)
  {
    vsol_point_2d_sptr p = new vsol_point_2d(x_pos[i], y_pos[i]);
    point_array[i].set_point(p);
    point_array[i].set_strength(val[i]);
  }
  vcl_qsort(point_array, n_corners, sizeof(sdet_harris_point),
            (int (*)(const void *, const void *))&compare);
  //output the corners (limit by maximum number of corners)
  int num = (int)(percent_corners_/100.0*n_corners);
  if (num>n_corners)
    num = n_corners;
  for (int i=0; i<num; i++)
  {
    points_.push_back(point_array[i].point());
    // vcl_cout <<"s[" << i << "]=" << point_array[i].strength() << '\n';
  }
  delete [] point_array;
  points_valid_ = true;
}

//----------------------------------------------------------
//: Clear internal storage
//
void sdet_harris_detector::clear()
{
  points_.clear();
  points_valid_ = false;
}

