#include "FundMatFitting.hpp"
  
  void FundMatFitting::setData(std::vector<cv::Point2d> pts1, std::vector<cv::Point2d> pts2){
  
      for (int i = 0; i < pts1.size(); i++){
          correspondences.push_back(Correspondence(pts1[i],pts2[i]));
      }
  }
  
  double FundMatFitting::estimErrorForSample(int i)
  {
      double x2 = correspondences[i].second.x;
      double y2 = correspondences[i].second.y;
      double x1 = correspondences[i].first.x;
      double y1 = correspondences[i].first.y;
  
      double error = (a*x2 + b*y2 +c*x1 + d*y1 + 1) / sqrt(a*a + b*b + c*c + d*d); //H&Z, p.350
      return error;
  }
  
  void FundMatFitting::estimModelFromSamples(std::vector<int> samplesIdx)
  {
      int nbPts = 0;
      nbPts = (int)samplesIdx.size();
  
      cv::Mat W = cv::Mat::zeros(4, nbPts,  cv::DataType<double>::type);
  
      for (int i = 0; i < nbPts; i++){
          W.at<double>(0,i) = correspondences[samplesIdx[i]].second.x;
          W.at<double>(1,i) = correspondences[samplesIdx[i]].second.y;
          W.at<double>(2,i) = correspondences[samplesIdx[i]].first.x;
          W.at<double>(3,i) = correspondences[samplesIdx[i]].first.y;
      }
  
      cv::Mat meanW = cv::Mat::zeros(4, 1, cv::DataType<double>::type);
  
      meanW.at<double>(0) = cv::mean(W.row(0))[0];
      meanW.at<double>(1) = cv::mean(W.row(1))[0];
      meanW.at<double>(2) = cv::mean(W.row(2))[0];
      meanW.at<double>(3) = cv::mean(W.row(3))[0];
  
      for (int i = 0; i < nbPts; i++){
          for (int j = 0; j < 4; j++){
              W.at<double>(j,i) = W.at<double>(j,i) - meanW.at<double>(j);
          }
      }
  
      cv::Mat S, U, Vt;
      cv::SVD::compute(W.t(), S, U, Vt);
      cv::Mat V = Vt.t();
      cv::Mat N = V.col(V.cols - 1);
  
      double e = cv::Mat(-1*N.t()*meanW).at<double>(0,0);
      a = N.at<double>(0) / e;
      b = N.at<double>(1) / e;
      c = N.at<double>(2) / e;
      d = N.at<double>(3) / e;
  }
  
  bool FundMatFitting::isDegenerate(std::vector<int> samplesIdx)
  {
      return false;
  }