#include "autostitch.h"

#include "meowpp/math/Matrix.h"
#include "meowpp/math/Vector.h"
#include <utility>
#include <cmath>
#include <cstdio>
#include <algorithm>


using namespace meow;

double MyRansacCheck::threshold = 5.0;

meow::Usage MyRansacCheck::usage() {
  Usage usg;
  usg.optionAdd("ransac-threshold",
                "Threshold for RANSAC",
                "<floating point>", stringPrintf("%.10f", threshold),
                false);
  return usg;
}

bool MyRansacCheck::usage(Usage const& usg) {
  threshold = inRange(0.0000001, 1000.0,
                      atof(usg.optionValue("ransac-threshold", 0).c_str()));
  return true;
}

MyRansacCheck::MyRansacCheck() {
}


MyRansacCheck::MyRansacCheck(MyRansacCheck const& __rc):
_from(__rc._from),
_to(__rc._to){
}


MyRansacCheck::MyRansacCheck(std::vector<Vector<double> > const* __from,
                             std::vector<Vector<double> > const* __to,
                             double w_max, double h_max,
                             double rr, double aa):
_from(__from),
_to(__to),
_w(w_max),
_h(h_max),
r_(rr), ang(aa) {
}


MyRansacCheck::~MyRansacCheck(){
}


Vector<double> MyRansacCheck::vCalc(std::vector<FeaturePointIndexPair> const& __sample) const {
  Matrix<double> m(__sample.size() * 2, 9, 0.0);
  for (size_t i = 0, I = __sample.size(); i < I; ++i) {
    m(i * 2    , 0, (*_from)[__sample[i].from.second](0));
    m(i * 2    , 1, (*_from)[__sample[i].from.second](1));
    m(i * 2    , 2, 1.0);
    m(i * 2    , 6, -(*_from)[__sample[i].from.second](0) * (*_to)[__sample[i].to.second](0));
    m(i * 2    , 7, -(*_from)[__sample[i].from.second](1) * (*_to)[__sample[i].to.second](0));
    m(i * 2    , 8,                                         (*_to)[__sample[i].to.second](0));
    m(i * 2 + 1, 3, (*_from)[__sample[i].from.second](0));
    m(i * 2 + 1, 4, (*_from)[__sample[i].from.second](1));
    m(i * 2 + 1, 5, 1.0);
    m(i * 2 + 1, 6, -(*_from)[__sample[i].from.second](0) * (*_to)[__sample[i].to.second](1));
    m(i * 2 + 1, 7, -(*_from)[__sample[i].from.second](1) * (*_to)[__sample[i].to.second](1));
    m(i * 2 + 1, 8,                                         (*_to)[__sample[i].to.second](1));
  }
  if (__sample.size() == 4) {
    m.triangulared();
    Vector<double> x(8, 0.0);
    for (ssize_t i = 7; i >= 0; i--) {
      double sum = 0;
      for (size_t j = i + 1; j < 8u; j++) {
        sum += x(j) * m(i, j);
      }
      x.scalar(i, (m(i, 8) - sum) / m(i, i));
    }
    return x;
  }
  else {
    Matrix<double> b(m.col(8));
    m.cols(8, 0.0);
    Vector<double> v((m.transpose() * m).inverse() * m.transpose() * b);
    return v;
  }
}


void MyRansacCheck::rememberVCalc(std::vector<FeaturePointIndexPair>
                                  const& __sample) {
  Vector<double> x(vCalc(__sample));
  a_ = x(0);
  b_ = x(1);
  c_ = x(2);
  d_ = x(3);
  e_ = x(4);
  f_ = x(5);
  A_ = x(6);
  B_ = x(7);
}


bool MyRansacCheck::ok(FeaturePointIndexPair const& __m) const {
  Vector2D<double> from(
    (*_from)[__m.from.second](0),
    (*_from)[__m.from.second](1));
  Vector2D<double> me(
    (*_to)[__m.to.second](0),
    (*_to)[__m.to.second](1));
  Vector2D<double> me2(to(from));
  return ((me - me2).length2() <= threshold);
}


double MyRansacCheck::operator()(std::vector<FeaturePointIndexPair>
                                 const& __sample,
                                 std::vector<FeaturePointIndexPair>
                                 const& __data) const {
  for(size_t i = 0, I = __sample.size(); i < I; i++) {
    for (size_t j = 0, J = __sample.size(); j < J; j++) {
      if(i == j) continue;
      if(__sample[i].from.second == __sample[j].from.second) return -1;
      if(__sample[i].to  .second == __sample[j].to  .second) return -1;
    }
  }
  ((MyRansacCheck*)this)->rememberVCalc(__sample);
  if (!((MyRansacCheck*)this)->check()) return -999;
  size_t ret = 0;
  for (size_t i = 0, I = __data.size(); i < I; i++) {
    if (ok(__data[i])) {
      ret++;
    }
  }
  return 0.001 + ret;
}

bool MyRansacCheck::check() {
  return true;
  Vector2D<double> v_x(a_, b_);
  Vector2D<double> v_y(c_, d_);
  double xx[2] = {0, _w}, yy[2] = {0, _h};
  for (size_t i = 0; i < 2; ++i) {
    for (size_t j = 0; j < 2; ++j) {
      if (A_ * xx[i] + B_ * yy[j] + 1 <= 0) return false;
    }
  }
  double len1 = v_x.length() * r_;
  double len2 = v_x.length() / r_;
  double len = v_y.length();
  if (len1 > len2) std::swap(len1, len2);
  if (len < len1 || len2 < len) return false;
  double sn = fabs(sin(ang));
  double msn = fabs(v_x.cross(v_y) / v_x.length() / v_y.length());
  if (msn < sn) return false;
  return true;
}

void MyRansacCheck::print() const {
  printf("%f %f %f   %f %f %f   %f %f 1\n", a_, b_, c_, d_, e_, f_, A_, B_);
}

Vector2D<double> MyRansacCheck::to(Vector2D<double> const& v) const {
  return Vector2D<double>(
    (v.x() * a_ + v.y() * b_ + c_) / (A_ * v.x() + B_ * v.y() + 1),
    (v.x() * d_ + v.y() * e_ + f_) / (A_ * v.x() + B_ * v.y() + 1)
  );
}