/**
 *  @brief Line fitting to a set of 2D points
 *
 *  @author  Andrey Kudryavtsev (avkudr.github.io)
 *  @author  Rahima Djahel (github:rahma24000)
 *  @date    27/03/2018
 *  @version 1.0
 */

#ifndef FUND_MAT_FITTING_H
#define FUND_MAT_FITTING_H

#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>

#include "robust_estim.hpp"

// ----------------------------------------------------------------------------
// ------- Class defining the Problem for RobustEstimator (RANSAC, LMedS)
// ------- for more details visit: /github.com/avkudr/robest
// ----------------------------------------------------------------------------

class FundMatFitting : public robest::EstimationProblem{
    typedef std::pair<cv::Point2d,cv::Point2d> Correspondence;
    typedef std::vector<Correspondence> CorrespondenceVector;
public:
    FundMatFitting(){
        setNbParams(4);//4 parameters a b c d
        setNbMinSamples(4);//at least 4 corresponndant points 
    }

    void setData(std::vector<cv::Point2d> pts1, std::vector<cv::Point2d> pts2);

    double estimErrorForSample(int i);
    void   estimModelFromSamples(std::vector<int> samplesIdx);

    int getTotalNbSamples() const{
        return (int) correspondences.size();
    }

    long double a,b,c,d,e; // Params

    cv::Mat getResult(){
        cv::Mat F = (cv::Mat_<double>(3,3) << 0,0,a,0,0,b,c,d,1);
        if ( (!T1.empty()) && (!T2.empty())){
            F = T2.t() * F * T1;
        }
        F = F / cv::norm(F); // replaced with the norm: Peter Sturm suggestion
        return F;
    }
private:
    bool isDegenerate(std::vector<int> samplesIdx);
    cv::Mat T1, T2;
    CorrespondenceVector correspondences; // Data
};

#endif // FUND_MAT_FITTING_H