#include "autostitch.h"

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

using namespace meow;

double MyRansacCheck::threshold = 5.0;

meow::Usage MyRansacCheck::usage(){
  Usage usg;
  usg.optionAdd('t',
                "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('t', 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):
_from(__from),
_to(__to){
}


MyRansacCheck::~MyRansacCheck(){
}


std::pair<Vector3D<double>, Vector3D<double> > MyRansacCheck::vCalc(
  std::vector<FeaturePointIndexPair> const& __sample
) const{
  Matrix<double> m(6, 7, 0.0);
  for(size_t i = 0; i < 3u; 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, (*_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, (*_to)[__sample[i].to.second](1));
  }
  m.triangulared();
  Vector<double> x(6, 0.0);
  for(ssize_t i = 5; i >= 0; i--){
    double sum = 0;
    for(size_t j = i + 1; j < 6u; j++){
      sum += x(j) * m(i, j);
    }
    x.entry(i, (m(i, 6) - sum) / m(i, i));
  }
  Vector3D<double> vX(x(0), x(1), x(2));
  Vector3D<double> vY(x(3), x(4), x(5));
  return std::pair<Vector3D<double>, Vector3D<double> >(vX, vY);
}


void MyRansacCheck::rememberVCalc(std::vector<FeaturePointIndexPair>
                                  const& __sample){
  std::pair<Vector3D<double>, Vector3D<double> > p(vCalc(__sample));
  _vX = p.first;
  _vY = p.second;
}


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);
  size_t ret = 0;
  for(size_t i = 0, I = __data.size(); i < I; i++){
    if(ok(__data[i])){
      ret++;
    }
  }
  return 0.001 + ret;
}


Vector2D<double> MyRansacCheck::to(Vector2D<double> const& __v) const{
  Vector3D<double> v(__v(0), __v(1), 1);
  return Vector2D<double>(v.dot(_vX), v.dot(_vY));
}