#ifndef ICP_H
#define ICP_H

/*
The main TrICP algorithm class. Designed to iteract with the GUI.
*/

#include <wx/wx.h>
#include "MyANN.h"
#include "Matrix.h"
#include "Point.h"
#include "ICPPoint.h"

using namespace std;

class ICP
{
public:
    ICP();

    void SetLTS(float a);
    void SetPoints(vector <Point> &P1, vector <Point> &P2, float dist_threshold);
	void SetMaxPoints(unsigned int max);
	void Seteps(float e);
    void SetwxTextCtrl(wxTextCtrl *t); // Used for m_text feedback from ICP
    void SetwxApp(wxApp *a); // Used for m_text feedback from ICP

    void Run(Matrix &Transform);
	double GetMSE();

private:
    void CalcOverlappingRegion(vector <Point> &P1, vector <Point> & P2, Point &start, Point &end);
    bool ByDistSq(const ICPPoint &a, const ICPPoint &b);

private:
	vector <ICPPoint> m_points1;
	vector <Point> m_points2;
	ANN m_ANN_points2;

    float m_LTS; // Least trimmed square percentage [0.0, 1.0]
	unsigned int m_max_points;
	float m_eps;
	double m_MSE;

	wxTextCtrl *m_text;
    wxApp *m_app; // pointer to the application so we can update the display every iteration

};

#endif
